update bug crash miss docking

config/hybrid_local_planner_params.yaml

@@ -31,8 +31,8 @@ HybridLocalPlanner:
 
     # GoalTolerance
 
-    xy_goal_tolerance: 0.1
-    yaw_goal_tolerance: 0.07
+    xy_goal_tolerance: 0.05
+    yaw_goal_tolerance: 0.05
 
     # Optimization
 

config/maker_sources.yaml

@@ -9,14 +9,14 @@ trolley:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được   
     timeout: 60.0
-    vel_x: 0.25
+    vel_x: 0.2
     vel_theta: 0.3
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -30,7 +30,7 @@ trolley:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 charger:
   plugins:
@@ -41,13 +41,13 @@ charger:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60
-    vel_x: 0.1
+    vel_x: 0.15
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 dock_station:
   plugins:
@@ -102,7 +102,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
     
   qrcode:
     maker_goal_frame: qr_trolley
@@ -116,7 +116,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 undock_station_2:
   plugins:
@@ -135,7 +135,7 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -149,4 +149,4 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4

config/pnkx_local_planner_params.yaml

@@ -48,7 +48,7 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.15    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
   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
@@ -106,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -143,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -180,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -117,7 +117,9 @@ namespace mkt_algorithm
        * @return lookahead point
        */
       std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
+      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, 
+                        const double &lookahead_dist, 
+                        robot_nav_2d_msgs::Path2D& global_plan);
 
       /**
        * @brief Prune global plan
@@ -194,11 +196,11 @@ namespace mkt_algorithm
 
       /**
        * @brief Generate Hermite trajectory
-       * @param pose
+       * @param path
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Generate Hermite quadratic trajectory
@@ -206,7 +208,7 @@ namespace mkt_algorithm
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Should rotate to path

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

@@ -279,6 +279,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
                                                         const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                                                         double &x_direction, double &y_direction, double &theta_direction)
 {
+  // robot::log_error("DEBUG STEP 2.0");
   if (!initialized_)
   {
     robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
@@ -288,7 +289,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
   {
     last_actuator_update_ = robot::Time::now();
   }
-
+  // robot::log_error("DEBUG STEP 3.0");
   std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
   if (footprint.size() > 1)
   {
@@ -311,14 +312,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
     this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
   }
-
+  // robot::log_error("DEBUG STEP 4.0");
   if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
   {
     robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
     return false;
   }
   this->getParams();
-
+  // robot::log_error("DEBUG STEP 5.0");
   frame_id_path_ = global_plan.header.frame_id;
   goal_ = goal;
   global_plan_ = global_plan;
@@ -329,14 +330,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__);
     return false;
   }
-
+  // robot::log_error("DEBUG STEP 6.0");
   double S = std::numeric_limits<double>::infinity();
   S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
                costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
   const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
   S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
   compute_plan_.poses.clear();
-
+  // robot::log_error("DEBUG STEP 7.0");
   if ((unsigned int)global_plan_.poses.size() == 2)
   {
     double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
@@ -360,6 +361,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     }
   }
+  // robot::log_error("DEBUG STEP 8.0");
 
   double lookahead_dist = this->getLookAheadDistance(velocity);
   transform_plan_.poses.clear();
@@ -368,6 +370,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
+  // robot::log_error("DEBUG STEP 9.0");
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -378,14 +381,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     } 
   }  
-
+  // robot::log_error("DEBUG STEP 10.0");
   x_direction = x_direction_;
   y_direction = y_direction_ = 0;
   theta_direction = theta_direction_;
-
+  // robot::log_error("DEBUG STATUS : %x, %x", (unsigned int)(compute_plan_.poses.size() > 1), journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution());
   if ((unsigned int)(compute_plan_.poses.size() > 1) && 
       journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution())
   {
+    // robot::log_error("DEBUG STEP 10.1");
     const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
     int index;
     for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
@@ -406,12 +410,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
   }
   else
   {
+    // robot::log_error("DEBUG STEP 11.1");
     try
     {
+      
       auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // robot::log_error("DEBUG STEP 11.2");
       auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      // robot::log_error("DEBUG STEP 11.2.1 carrot_pose_it: %d", (int)std::distance(transform_plan_.poses.begin(), carrot_pose_it));
       double distance_it = 0;
-      for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
+
+      auto it = carrot_pose_it == transform_plan_.poses.begin() ? transform_plan_.poses.end() : carrot_pose_it - 1;
+
+      for ( ; it != transform_plan_.poses.begin(); --it)
       {
         double dx = it->pose.x - carrot_pose_it->pose.x;
         double dy = it->pose.y - carrot_pose_it->pose.y;
@@ -422,19 +433,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
           break;
         }
       }
+      // robot::log_error("DEBUG STEP 11.3");
 
       robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
                                       ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
                                       : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
       robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
-
+      // robot::log_error("DEBUG STEP 11.4");
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
       // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
       const double dir_path = 0.0;
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
+      // robot::log_error("DEBUG STEP 11.5");
     }
     catch (std::exception &e)
     {
@@ -442,7 +455,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     }
   }
-
+  // robot::log_error("DEBUG STEP 11.0");
   x_direction_ = x_direction;
   y_direction_ = y_direction;
   theta_direction_ = theta_direction;
@@ -476,7 +489,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     twist = traj_->nextTwist();
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  // drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  drive_cmd.x = sqrt(twist.x * twist.x);
+  
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
@@ -512,7 +527,7 @@ 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 &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -528,27 +543,23 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    // === Final Heading Alignment Check ===
-    double xy_error = 0.0, heading_error = 0.0;
-    if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
-    {
-      // Use Arc Motion controller for final heading alignment
-      alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
-#ifdef BUILD_WITH_ROS
-      ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                        heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
-    }
-    else
-    {
-      // if(fabs(carrot_pose.pose.y) > 0.2)
-      // {
-      //   lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
-      // }
-      robot_nav_2d_msgs::Twist2D drive_target;
+//     // === Final Heading Alignment Check ===
+//     double xy_error = 0.0, heading_error = 0.0;
+//     if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+//     {
+//       // Use Arc Motion controller for final heading alignment
+//       alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+// #ifdef BUILD_WITH_ROS
+//       ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//                         heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+// #endif
+//     }
+//     else
+//     {
+      robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
       transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
+      
       // Normal Pure Pursuit
       this->computePurePursuit(
       carrot_pose, 
@@ -559,7 +570,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       sign_x, 
       dt, 
       drive_cmd);
-    }
+    // }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
     if (this->nav_stop_)
     {
@@ -585,6 +596,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     break;
   }
 
+  if(fabs(v_max == 0.0))
+    drive_cmd.x = 0.0;
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -630,8 +643,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   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 + std::copysign(carrot_pose.pose.theta * dt, kappa);
+  double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
     if (trajectory.poses.size() >= 2) {
@@ -640,18 +655,22 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       for(int i = trajectory.poses.size() - 2; i >= 0; i--)
       {
         const auto& p = trajectory.poses[i].pose;
-        if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
+        const auto& dx =  p1.x - p.x ;
+        const auto& dy =  p1.y - p.y ;
+        if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
         {
-          heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
+          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 = angles::normalize_angle(M_PI + heading_ref);
-          }
+          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
         }
       }
 
-      const double error = angles::normalize_angle(heading_ref);
+      const double error = heading_ref;
+      ss << error << " ";
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -663,11 +682,12 @@ 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;
     }
     else
     {
-      w_target = 0.0;
+      w_target = std::clamp(w_target, -0.001, 0.001);
       near_goal_heading_was_active_ = false;
     }
   }
@@ -675,6 +695,7 @@ 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)
@@ -684,7 +705,6 @@ 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;
 
@@ -704,6 +724,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], -max_vel_theta_, max_vel_theta_);
+  // robot::log_info("%s", ss.str().c_str());
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -767,16 +788,22 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
     for(int i = 2; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
+      const auto& dx = p.pose.x - p1.pose.x;
+      const auto& dy = p.pose.y - p1.pose.y;
       if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
       {
-        path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x);
+        if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
+          continue;
+        path_angle = std::atan2(dy, dx);
         break;
       }
     }
   }
   
   // Whether we should rotate robot to rough path heading
-  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
+  if(sign_x < 0.0)
+    path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  angle_to_path = path_angle;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
   double heading_rotate = rotate_to_heading_min_angle_;
@@ -789,16 +816,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-// #ifdef BUILD_WITH_ROS
-  // if (result)
-  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+#ifdef BUILD_WITH_ROS
+  if (result)
+    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
-// #endif
+#endif
   return result;
 }
 
@@ -880,13 +907,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
     for(int i = trajectory.poses.size() - 2; i >= 0; i--)
     {
       const auto& p = trajectory.poses[i].pose;
-      if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+      const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+      if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
       {
-        heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
-        if(sign_x < 0.0)
-        {
-          heading_error = angles::normalize_angle(M_PI + heading_error);
-        }
+        heading_error = angles::normalize_angle(std::atan2(dy, dx));
         break;
       }
     }
@@ -1061,7 +1086,8 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
 }
 
 std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
+mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, 
+  const double &lookahead_dist, robot_nav_2d_msgs::Path2D& global_plan)
 {
   if (global_plan.poses.empty())
     throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
@@ -1098,9 +1124,104 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
   if (goal_pose_it == global_plan.poses.end())
     goal_pose_it = std::prev(global_plan.poses.end());
 
+  // --- Final safety check ---
+  if (goal_pose_it < global_plan.poses.begin() || goal_pose_it >= global_plan.poses.end())
+  {
+    // fallback cuối cùng
+    goal_pose_it = std::prev(global_plan.poses.end());
+  }
+
   return goal_pose_it;
 }
 
+// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
+// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(
+//     const robot_nav_2d_msgs::Twist2D &velocity,
+//     const double &lookahead_dist,
+//     robot_nav_2d_msgs::Path2D &global_plan)
+// {
+//   auto &poses = global_plan.poses;
+
+//   // --- Guard ---
+//   if (poses.empty())
+//     throw robot_nav_core2::PlannerTFException("The global plan is empty.");
+
+//   if (poses.size() == 1)
+//     return poses.begin();
+
+//   if (poses.size() == 2)
+//     return std::prev(poses.end());
+
+//   // --- Init ---
+//   size_t goal_index = poses.size() - 1;
+
+//   const auto &p0 = poses[0].pose;
+//   const auto &p1 = poses[1].pose;
+
+//   double start_angle = atan2(p1.y - p0.y, p1.x - p0.x);
+//   double turn_threshold = M_PI_2 * 0.6;
+
+//   // --- Detect turn ---
+//   for (size_t i = 1; i < poses.size(); ++i)
+//   {
+//     const auto &a = poses[i - 1].pose;
+//     const auto &b = poses[i].pose;
+
+//     double current_angle = atan2(b.y - a.y, b.x - a.x);
+//     double delta = angles::normalize_angle(current_angle - start_angle);
+
+//     goal_index = i;
+
+//     if (fabs(delta) >= turn_threshold)
+//       break;
+//   }
+
+//   // --- Clamp goal_index ---
+//   if (goal_index >= poses.size())
+//     goal_index = poses.size() - 1;
+
+//   // --- Safe search range ---
+//   auto search_begin = poses.begin();
+
+//   // ❗ IMPORTANT: +1 để iterator hợp lệ
+//   auto search_end = poses.begin() + goal_index + 1;
+
+//   if (search_end > poses.end())
+//     search_end = poses.end();
+
+//   // --- Find lookahead ---
+//   double accumulated_dist = 0.0;
+//   auto goal_pose_it = search_begin;
+//   for (auto it = search_begin + 1; it != search_end; ++it)
+//   {
+//     double dx = it->pose.x - std::prev(it)->pose.x;
+//     double dy = it->pose.y - std::prev(it)->pose.y;
+//     accumulated_dist += std::hypot(dx, dy);
+//     if (accumulated_dist >= lookahead_dist)
+//     {
+//       goal_pose_it = it;
+//       break;
+//     }
+//   }
+
+//   // --- Fallback an toàn ---
+//   if (goal_pose_it == search_begin)
+//   {
+//     goal_pose_it = std::prev(search_end); // safe vì search_end > search_begin
+//   }
+
+//   // --- Final safety check ---
+//   if (goal_pose_it < poses.begin() || goal_pose_it >= poses.end())
+//   {
+//     // fallback cuối cùng
+//     return std::prev(poses.end());
+//   }
+
+//   return goal_pose_it;
+// }
+
+
+
 bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot)
 {
   if (global_plan.poses.empty())
@@ -1236,17 +1357,17 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
 
   if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ )
     {
       return generateParallelPath(path, sign_x);
     }
-    return generateHermiteTrajectory(path.poses.back(), sign_x);
+    return generateHermiteTrajectory(path, sign_x);
   }
   else // nếu đường cong
   {
     if(fabs(drive_cmd.x) < min_speed_xy_)
       drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
-    return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
+    return generateHermiteQuadraticTrajectory(path, sign_x);
   }
 }
 
@@ -1276,40 +1397,49 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
       dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
       dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
     }
-
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double theta = atan2(dy, dx);
     double x_off = p.x - offset_y * sin(theta)*sign_x;
     double y_off = p.y - offset_y * cos(theta)*sign_x;
 
+    parallel_path.poses[i].header = path.poses[i].header;
     parallel_path.poses[i].pose.x = x_off;
     parallel_path.poses[i].pose.y = y_off;
-    parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
-    parallel_path.poses[i].header = path.poses[i].header;
+    parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
   }
-
   return parallel_path;
 }
 
+
+
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D hermite_trajectory;
   hermite_trajectory.poses.clear();
-  hermite_trajectory.header.stamp = pose.header.stamp;
-  hermite_trajectory.header.frame_id = pose.header.frame_id;
+  hermite_trajectory.header = path.header;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = pose.pose.theta;
+  if (path.poses.empty())
+    return hermite_trajectory;
+
+  const auto &goal = path.poses.back();
+  if (hermite_trajectory.header.frame_id.empty())
+    hermite_trajectory.header.frame_id = goal.header.frame_id;
+  if (hermite_trajectory.header.stamp.isZero())
+    hermite_trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  double theta = goal.pose.theta;
   const double L = std::hypot(x, y);
-
   if (L < 1e-6) {
     robot_nav_2d_msgs::Pose2DStamped pose_stamped;
-    pose_stamped.pose.x = 0.0;
-    pose_stamped.pose.y = 0.0;
-    pose_stamped.pose.theta = 0.0;
-    pose_stamped.header.stamp = pose.header.stamp;
-    pose_stamped.header.frame_id = pose.header.frame_id;
+    pose_stamped.pose.x = x;
+    pose_stamped.pose.y = y;
+    pose_stamped.pose.theta = theta;
+    pose_stamped.header.stamp = hermite_trajectory.header.stamp;
+    pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
     hermite_trajectory.poses.push_back(pose_stamped);
     return hermite_trajectory;
   }
@@ -1343,30 +1473,39 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
     double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
 
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
-    robot_nav_2d_msgs::Pose2DStamped pose;
-    pose.pose.x = px;
-    pose.pose.y = py;
-    pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
-    pose.header.stamp = hermite_trajectory.header.stamp;
-    pose.header.frame_id = hermite_trajectory.header.frame_id;
-    hermite_trajectory.poses.push_back(pose);
+    robot_nav_2d_msgs::Pose2DStamped pose_out;
+    pose_out.pose.x = px;
+    pose_out.pose.y = py;
+    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
+    pose_out.header.stamp = hermite_trajectory.header.stamp;
+    pose_out.header.frame_id = hermite_trajectory.header.frame_id;
+    hermite_trajectory.poses.push_back(pose_out);
   }
   return hermite_trajectory;
 }
 
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D trajectory;
   trajectory.poses.clear();
-  trajectory.header.stamp = pose.header.stamp;
-  trajectory.header.frame_id = pose.header.frame_id;
+  trajectory.header = path.header;
+  if (path.poses.empty())
+    return trajectory;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
+  const auto &goal = path.poses.back();
+  if (trajectory.header.frame_id.empty())
+    trajectory.header.frame_id = goal.header.frame_id;
+  if (trajectory.header.stamp.isZero())
+    trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta;
   const double L = std::hypot(x, y);
   if (L < 1e-6)
   {
@@ -1406,6 +1545,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
 
     double dx = 2.0 * ax * t + bx;
     double dy = 2.0 * ay * t + by;
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
     robot_nav_2d_msgs::Pose2DStamped pose_out;

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -70,9 +70,8 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
       double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
       if(fabs(tolerance) <= xy_goal_tolerance_) 
       {
-        robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
-        robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
-        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -176,7 +176,9 @@ namespace two_points_planner
     const double dy = goal.pose.position.y - start.pose.position.y;
     const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
-
+    // Lấy độ phân giải của costmap
+    double resolution = costmap_robot_->getCostmap()->getResolution();
+    
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -188,17 +190,18 @@ namespace two_points_planner
     }
     else 
     { 
-      robot_geometry_msgs::PoseStamped pose = start;
-      pose.pose.position.x += 0.01 * cos(theta);
-      pose.pose.position.y += 0.01 * sin(theta);
+      auto goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);  // hoặc start.theta
+      robot_geometry_msgs::PoseStamped pose = goal;
+      pose.pose.position.x += resolution * std::cos(goal_2d.pose.theta);
+      pose.pose.position.y += resolution * std::sin(goal_2d.pose.theta);
+      plan.push_back(pose);
+      pose = goal;
+      pose.pose.position.x -= resolution * std::cos(goal_2d.pose.theta);
+      pose.pose.position.y -= resolution * std::sin(goal_2d.pose.theta);
       plan.push_back(pose);
       plan.push_back(goal);
       return true;
     }
-
-    // Lấy độ phân giải của costmap
-    double resolution = costmap_robot_->getCostmap()->getResolution();
-
     // Tính số điểm cần chia
     int num_points = std::ceil(distance / resolution);
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -31,6 +31,8 @@ namespace pnkx_local_planner
     void initialize(robot::NodeHandle &parent, const std::string &name,
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
+    void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
+
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -240,6 +240,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &n
   }
 }
 
+void pnkx_local_planner::PNKXDockingLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
+{
+  this->reset();
+  pnkx_local_planner::PNKXLocalPlanner::setPlan(path);
+}
+
 void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 {
   robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received.");
@@ -250,21 +256,17 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
-  robot::log_info_at(__FILE__, __LINE__, "Debug");
-  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
 
   parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  nh_algorithm.setParam("allow_rotate", false);
-
-  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
-  parent_.printParams();
+  // nh_algorithm.setParam("allow_rotate", false);
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {  
+  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
   this->getParams(planner_nh_);
   if (update_costmap_before_planning_)
   {
@@ -278,11 +280,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
-
+  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2.0");
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                              local_goal_pose = this->transformPoseToLocal(goal_pose_);
+  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3.0");
   if (start_docking_)
   {
     local_goal_pose = goal_pose_;
@@ -290,6 +293,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
+    // robot::log_error("local_start_pose (%f, %f, %f)", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
+    // robot::log_error("local_goal_pose (%f, %f, %f)", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
+    // for(size_t i = 0; i < global_plan_.poses.size(); i++)
+    // {
+    //   robot::log_error("global_plan_ [%zu] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
+    // }
+    // robot::log_error("costmap_robot_->getGlobalFrameID(): %s", costmap_robot_->getGlobalFrameID().c_str());
+    
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
@@ -299,53 +310,60 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!ret_nav_)
+  if (!ret_nav_ && !dkpl_.empty())
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 4.0");
+    if(nav_algorithm_)
     {
-      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
-      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
-    }
-    // else
-    //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
-
-    if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
-    {
-      this->lock();
-      robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
-      traj_generator_->setTwistLinear(linear);
-      linear.x *= (-1);
-      traj_generator_->setTwistLinear(linear);
-      traj_generator_->setTwistAngular(dkpl_.front()->angular_);
-
-      std::string algorithm_nav_name;
-      planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-      nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
-      nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
-      nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
-      nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
-      nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
-
-      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
-
-      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
+      if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
       {
+        robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
+        throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      }
+      // else
+      //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
+      // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 5.0");
+      if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
+      {
+        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 6.0");
+        this->lock();
+        robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
+        traj_generator_->setTwistLinear(linear);
+        linear.x *= (-1);
+        traj_generator_->setTwistLinear(linear);
+        traj_generator_->setTwistAngular(dkpl_.front()->angular_);
 
-        if (dkpl_.front()->following_)
+        std::string algorithm_nav_name;
+        planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+        robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+        nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
+        nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
+        nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
+        nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
+        nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
+        
+        parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+        parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 7.0");
+        if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
         {
-          robot_nav_2d_msgs::Pose2DStamped follow_pose;
-          if (dkpl_.front()->geLocalGoal(local_goal_pose))
+
+          if (dkpl_.front()->following_)
           {
-            local_goal_pose = follow_pose;
+            robot_nav_2d_msgs::Pose2DStamped follow_pose;
+            if (dkpl_.front()->geLocalGoal(local_goal_pose))
+            {
+              local_goal_pose = follow_pose;
+            }
+          }
+          if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
+          {
+            throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
+            robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
           }
         }
-        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
-        {
-          throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
-          robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
-        }
+        
+        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 8.0");
       }
     }
   }
@@ -370,14 +388,26 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
                                                                                                  const robot_nav_2d_msgs::Twist2D &velocity)
 {
-  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  // // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  // for(size_t i = 0; i < global_plan_.poses.size(); i++)
+  // {
+  //   robot::log_error("computeVelocityCommands global_plan_ [%d] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
+  // }
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
+  cmd_vel.velocity.x = 0.0;
+  cmd_vel.velocity.y = 0.0;
+  cmd_vel.velocity.theta = 0.0;
   try
   {
-    if (global_plan_.poses.empty())
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
+    if ( global_plan_.poses.empty())
       return cmd_vel;
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.11");
     this->prepare(pose, velocity);
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2");
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3");
     return cmd_vel;
   }
   catch (const robot_nav_core2::PlannerException &e)
@@ -424,6 +454,11 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
 
 bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
+  // if(global_plan_.poses.size() <= 2) 
+  // {
+  //   robot::log_error("DEBUG GOAL");
+  //   return true;
+  // }
   if (goal_pose_.header.frame_id == "")
   {
     robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
@@ -455,11 +490,11 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     {
       robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
       robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
-      robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
     }
   }
 
-  if (ret_nav_ && !ret_angle_ && !dock_ok)
+  // if (ret_nav_ && !ret_angle_ && !dock_ok)
+  if (ret_nav_ && !ret_angle_)
   {
     double delta_orient =
         fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
@@ -516,6 +551,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
                 dkpl_.front()->getLocalPath(local_pose, local_goal, path);
                 this->setPlan(robot_nav_2d_utils::pathToPath(path));
                 this->setGoalPose(local_goal);
+                robot::log_debug(__FILE__, __LINE__, "DEBUG 1 size path: %d", (int)path.poses.size());
               }
             }
             catch (const std::exception &e)
@@ -540,6 +576,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
                 path.poses.push_back(local_goal);
                 this->setPlan(path);
                 this->setGoalPose(local_goal);
+                robot::log_debug(__FILE__, __LINE__, "DEBUG 2 size path: %d", (int)path.poses.size());
               }
             }
             catch (const std::exception &e)
@@ -548,6 +585,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             }
           }
         }
+        
       }
       else
       {
@@ -801,7 +839,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
   robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
   robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
   std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
-
+  robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
+  robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
     throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -146,7 +146,7 @@ namespace robot_nav_core
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) {return;}
 
     /**
      * @brief  Constructs the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -103,7 +103,7 @@ namespace robot_nav_core2
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
+    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) {return;}
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -976,6 +976,7 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
+
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1116,7 +1117,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
   }
   catch (const std::exception &e)
@@ -1424,7 +1424,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
   }
   catch (const std::exception &e)
@@ -2496,6 +2495,7 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
     // the real work on pursuing a goal is done here
     bool done = executeCycle(goal);
 
+    // robot::log_debug("[MoveBase] Completed an execution cycle: ̀done=%s", done ? "true" : "false");
     // if we're done, then we'll return from execute
     if (done)
       return;
@@ -2715,7 +2715,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     controller_plan_ = latest_plan_;
     latest_plan_ = temp_plan;
     lock.unlock();
-    robot::log_debug("pointers swapped!");
+    robot::log_debug("pointers swapped!: %d", controller_plan_->size());
 
     if (!tc_->setPlan(*controller_plan_))
     {
@@ -2736,6 +2736,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       // as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
       return true;
     }
+    // robot::log_debug("pointers swapped2!");
 
     // make sure to reset recovery_index_ since we were able to find a valid plan
     if (recovery_trigger_ == PLANNING_R)
@@ -2747,6 +2748,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
   if (cancel_ctr_ && tc_)
   {
     robot_geometry_msgs::Vector3 linear;
+    linear.x = 0.0;
+    linear.y = 0.0;
+    linear.z = 0.0;
     // ROS_INFO_THROTTLE(1.0,"MoveTo is Canling ....");
     tc_->setTwistLinear(linear);
     try
@@ -2916,10 +2920,12 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
     {
       boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+      // robot::log_error("paused_: %s", paused_ ? "true" : "false");
       if (!paused_)
       {
         if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
         {
+          // robot::log_debug("Got a valid velocity command from the local planner start!");
           robot_nav_msgs::Path path;
           tc_->getPlan(path.poses);
           if (!path.poses.empty())
@@ -2945,6 +2951,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           }
           if (recovery_trigger_ == CONTROLLING_R)
             recovery_index_ = 0;
+          // robot::log_debug("Got a valid velocity command from the local planner end!");
         }
         else
         {
@@ -3144,15 +3151,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
   }
 
   std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
-  // robot_geometry_msgs::PoseStamped goal_pose, global_pose;
-  // goal_pose = goal_pose_msg;
-
-  // goal_pose.header.stamp = robot::Time(); // latest available
-  // try
-  // {
-  //   tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
-  //   tf3::doTransform(goal_pose, global_pose, transform);
-  // }
   robot_geometry_msgs::PoseStamped global_pose;
   tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
   robot_geometry_msgs::PoseStamped goal_pose;
@@ -3218,17 +3216,16 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
   {
     robot_nav_msgs::Path path;
     tc_->getGlobalPlan(path.poses);
-    if (!path.poses.empty())
+    robot_nav_msgs::Path global_path;
+    global_path.header.stamp = robot::Time::now();
+    global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
+    for(auto &p : path.poses)
     {
-      path.header.stamp = path.poses[0].header.stamp;
-      path.header.frame_id = path.poses[0].header.frame_id;
+      robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
+      pose.header.stamp = robot::Time::now();
+      global_path.poses.push_back(goalToGlobalFrame(pose));
     }
-    else
-    {
-      path.header.stamp = robot::Time::now();
-      path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
-    }
-    global_data_.plan = robot_nav_2d_utils::pathToPath(path);
+    global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
   }
   ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
   convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);