update

config/move_base_common_params.yaml

@@ -7,7 +7,7 @@ base_global_planner: CustomPlanner
 
 PNKXLocalPlanner:
   base_local_planner: LocalPlannerAdapter
-  base_global_planner: CustomPlanner
+  base_global_planner: TwoPointsPlanner  
 
 PNKXDockingLocalPlanner:
   base_local_planner: LocalPlannerAdapter
@@ -26,11 +26,11 @@ controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
 planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
-max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
+max_planning_retries: -1     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
 oscillation_distance: 0.5
 ### recovery behaviors
-recovery_behavior_enabled: true
+recovery_behavior_enabled: false 
 recovery_behaviors: [       
                       {name: aggressive_reset, type: ClearCostmapRecovery},
                       {name: conservative_reset, type: ClearCostmapRecovery},                      

config/pnkx_local_planner_params.yaml

@@ -1,5 +1,5 @@
 yaw_goal_tolerance: 0.03  
-xy_goal_tolerance: 0.02 
+xy_goal_tolerance: 0.05 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:

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

@@ -443,8 +443,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      return false;
+      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      x_direction = x_direction_; 
     }
   }
 
@@ -1262,11 +1262,12 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   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) 
   { 
-    if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
+
+    if(hypot(path.poses.back().pose.x, path.poses.back().pose.y) * 0.9 < min_lookahead_dist_)
     {
       if(fabs(path.poses.back().pose.x) < min_journey_squared_)
         drive_cmd.theta = 0.01;
@@ -1326,8 +1327,6 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
   return parallel_path;
 }
 
-
-
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
   const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {