Tạm bỏ

config/pnkx_local_planner_params.yaml

@@ -1,5 +1,5 @@
 yaw_goal_tolerance: 0.02  
-xy_goal_tolerance: 0.02 
+xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
@@ -95,7 +95,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
   min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.35                    # Minimum squared journey to consider for goal (default: 0.2) 
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
   max_journey_squared: 0.5                    # Maximum squared journey to consider for goal (default: 0.2) 
   max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 

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

@@ -206,7 +206,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // Process index_s with multiple elements
     if (index_s.size() > 1)
     {
-      for (size_t i = 0; i < index_s.size(); ++i)
+      for (size_t i = 1; i < index_s.size(); ++i)
       {
         if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
         {

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

@@ -520,8 +520,8 @@ 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;
-  robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
-  allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 6.0;
+  // robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
+  // allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0;
 
   allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);