update

config/pnkx_local_planner_params.yaml

@@ -56,9 +56,9 @@ LimitedAccelGenerator:
   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_x: -3.0
   decel_lim_y: -0.0
-  decel_lim_theta: -3.0
+  decel_lim_theta: -2.0
 
   # Whether to split the path into segments or not
   split_path: true
@@ -74,7 +74,7 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
@@ -119,7 +119,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
@@ -156,7 +156,7 @@ MKTAlgorithmDiffGoStraight:
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true

src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp

@@ -219,11 +219,12 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
         double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
 
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.3)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
+
             if (index_s[i] > sub_goal_index_saved_)
             {
               sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
@@ -257,7 +258,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
         double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
         double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)

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

@@ -520,9 +520,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-  allow_rotate |= fabs(transformed_plan.poses.back().pose.theta) >= M_PI / 2;
+  robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
-  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);
+  allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 6.0;
 
+  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);
 
   double angle_to_heading; 
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
@@ -624,15 +625,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   // 3) Adjust speed using Hermite trajectory curvature + remaining distance
   double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
-  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
+  // const double L_min = 0.1;  // m, chỉnh theo nhu cầu
-  double scale_close = std::clamp(L / L_min, 0.0, 1.0);
+  // double scale_close = std::clamp(L / L_min, 0.0, 1.0);
-  v_target *= scale_close;
+  // v_target *= scale_close;
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
   {
     double scale = y_soft / y_abs;  // y càng lớn => scale càng nhỏ
-    scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
+    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
     v_target *= scale;
     robot_nav_2d_msgs::Twist2D cmd, result;
     cmd.x = v_target;