update reduce speed

config/pnkx_local_planner_params.yaml

@@ -53,12 +53,12 @@ LimitedAccelGenerator:
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.5
+  acc_lim_x: 3.0
   acc_lim_y: 0.0      # diff drive robot
   acc_lim_theta: 1.5
   decel_lim_x: -1.5
   decel_lim_y: -0.0
-  decel_lim_theta: -1.5
+  decel_lim_theta: -3.0
 
   # Whether to split the path into segments or not
   split_path: true

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -522,9 +522,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   allow_rotate |= 
   (path_distance_to_rotate >= distance_allow_rotate && 
   fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)) <= 0.1);
-  // allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
+
-  // allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1;
-  // robot::log_info("%f front.y %f", path_distance_to_rotate, fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)));
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -581,6 +579,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
+    
   }
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
@@ -643,9 +642,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   // 4) Maintain minimum approach speed
   if (std::fabs(v_target) < min_approach_linear_velocity)
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
-
-  std::stringstream ss;
-
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -663,7 +659,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
           if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
             continue;
           heading_ref = std::atan2(dy, dx);
-          ss << "error " << heading_ref << " ";
           if(sign_x < 0.0)
           heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
@@ -671,7 +666,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       }
 
       const double error = heading_ref;
-      ss << error << " ";
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -683,7 +677,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
         w_heading);
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
-      ss << "dw_heading " << dw_heading << " ";
       w_target = velocity.theta + dw_heading;
       w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
@@ -697,7 +690,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   {
     near_goal_heading_was_active_ = false;
   }
-  
   w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
@@ -706,7 +698,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-
+  
   Eigen::VectorXd y(2);
   y << drive_cmd.x, drive_cmd.theta;
 
@@ -726,7 +718,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
   if (kf_filter_angular_)
     drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
-  robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
+  // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1261,11 +1253,11 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
+
+  drive_cmd.x = drive_target.x;
+  drive_cmd.theta = max_vel_theta_;
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
-  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
-  drive_cmd.theta = max_vel_theta_;
-  
   // nếu đường thẳng
   if (max_kappa <= straight_threshold) 
   {