Tạm bỏ

CATKIN_BUILD_README.md

@@ -20,7 +20,11 @@ The specified base path contains a CMakeLists.txt but "catkin_make" must be invo
 
 # Build trong workspace mới
 cd ../pnkx_nav_catkin_ws
-catkin_make
+rm -rf build devel
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo \
+  -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_C_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address"
 source devel/setup.bash
 ```
 

CMAKE_BUILD_README.md

@@ -356,8 +356,7 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config
 
 # Chỉ định workspace directory
 export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core
-
+ ] không install)
-# LD_LIBRARY_PATH (nếu không install)
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib
 ```
 

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/move_base_common_params.yaml

@@ -30,7 +30,7 @@ max_planning_retries: 0     # ... 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  
+yaw_goal_tolerance: 0.02  
-xy_goal_tolerance: 0.02 
+xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
@@ -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_x: -3.0
   decel_lim_y: -0.0
-  decel_lim_theta: -1.5
+  decel_lim_theta: -2.0
 
   # Whether to split the path into segments or not
   split_path: true
@@ -74,8 +74,8 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
-  angle_threshold: 0.47
+  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 
@@ -106,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
+  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -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
@@ -143,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
+  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -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
@@ -180,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
+  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1

examples/NavigationExample/NavigationExample.csproj

@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net10.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>

src/APIs/c_api/src/nav_c_api.cpp

@@ -83,6 +83,7 @@ extern "C" NavigationHandle navigation_create(void)
     // Using default constructor - initialization will be done via navigation_initialize()
     robot::PluginLoaderHelper loader;
     std::string path_file_so = loader.findLibraryPath("MoveBase");
+    robot::log_warning("%s", path_file_so.c_str());
     auto move_base_loader = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
         path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
     auto move_base_ptr = move_base_loader();
@@ -568,10 +569,16 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
       return false;
 
     robot_geometry_msgs::Vector3 linear;
-    linear.x = linear_x;
+    linear.x = 0.1;
     linear.y = linear_y;
     linear.z = linear_z;
-    return nav_ptr->setTwistLinear(linear);
+    bool result = nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Forward %f", linear.x);
+    
+    linear.x = -0.1;
+    result &= result && nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Backward %f", linear.x);
+    return result;
   }
   catch (...)
   {

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())
         {
@@ -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.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_)
           {
+
             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_)
@@ -415,7 +416,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   robot_nav_2d_msgs::Pose2DStamped sub_pose;
   sub_pose = global_plan.poses[closet_index];
 
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
     geometry_msgs::PoseStamped sub_pose_stamped_ros;
@@ -423,7 +424,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
     sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
     sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
-    sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
+    sub_pose_stamped_ros.pose.position.z = 0.9;
     sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
     sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
     sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
@@ -434,7 +435,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
 
   robot_nav_2d_msgs::Pose2DStamped sub_goal;
   sub_goal = global_plan.poses[goal_index];
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
     geometry_msgs::PoseStamped sub_goal_stamped_ros;
@@ -442,7 +443,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
     sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
     sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
-    sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
+    sub_goal_stamped_ros.pose.position.z = 0.9;
     sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
     sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
     sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;

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

@@ -181,7 +181,7 @@ namespace mkt_algorithm
        */
       robot_nav_2d_msgs::Path2D generateTrajectory(  
         const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
-        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);
 
       /**
        * @brief Generate trajectory
@@ -194,11 +194,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 +206,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_go_straight.cpp

@@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
   robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
   
-  // === Final Heading Alignment Check ===
+//   // === Final Heading Alignment Check ===
-  double xy_error = 0.0, heading_error = 0.0;
+//   double xy_error = 0.0, heading_error = 0.0;
-  if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+//   if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
-  {
+//   {
-    // Use Arc Motion controller for final heading alignment
+//     // Use Arc Motion controller for final heading alignment
-    alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+//     alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
-#ifdef BUILD_WITH_ROS
+// #ifdef BUILD_WITH_ROS
-    ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//     ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                      xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+//                       xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
+// #endif
-  }
+//   }
-  else
+//   else
-  {
+//   {
     // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", 
     //   journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
     if(fabs(carrot_pose.pose.y) > 0.2)
@@ -151,7 +151,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
       lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
     }
     robot_nav_2d_msgs::Twist2D drive_target;
-    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
     // Normal Pure Pursuit
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     sign_x, 
     dt, 
     drive_cmd);
-  }
+  // }
   applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
 
   if (this->nav_stop_)

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

@@ -368,6 +368,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;
   }
+
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -404,42 +405,47 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     
   }
-  else
+  else if(compute_plan_.poses.size() == 1)
   {
     try
     {
-      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
-      auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      // auto prev_carrot_pose_it = transform_plan_.poses.begin();
-      double distance_it = 0;
+      // double distance_it = 0;
-      for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
+      // for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
-      {
+      // {
-        double dx = it->pose.x - carrot_pose_it->pose.x;
+      //   double dx = it->pose.x - carrot_pose_it->pose.x;
-        double dy = it->pose.y - carrot_pose_it->pose.y;
+      //   double dy = it->pose.y - carrot_pose_it->pose.y;
-        distance_it += std::hypot(dx, dy);
+      //   distance_it += std::hypot(dx, dy);
-        if (distance_it > costmap_robot_->getCostmap()->getResolution())
+      //   if (distance_it > costmap_robot_->getCostmap()->getResolution())
-        {
+      //   {
-          prev_carrot_pose_it = it;
+      //     prev_carrot_pose_it = it;
-          break;
+      //     break;
-        }
+      //   }
-      }
+      // }
 
-      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
+      // 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((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
+      //                                 : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
-      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
+      // robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
       // 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;
+
+      
+      auto goal_pose = compute_plan_.poses.front().pose;
+      auto start_pose = pose.pose;
+      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
+      double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      return false;
+      x_direction = x_direction_; 
     }
   }
 
@@ -477,6 +483,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   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 = sqrt(twist.x * twist.x);
+  
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
@@ -512,6 +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;
+  // 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);
 
   double angle_to_heading; 
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
@@ -528,25 +540,21 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    // === Final Heading Alignment Check ===
+//     // === Final Heading Alignment Check ===
-    double xy_error = 0.0, heading_error = 0.0;
+//     double xy_error = 0.0, heading_error = 0.0;
-    if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+//     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);
+//       // 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
 //     }
-      robot_nav_2d_msgs::Twist2D drive_target;
+//     else
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+//     {
+      robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
       
       // Normal Pure Pursuit
@@ -559,7 +567,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_)
     {
@@ -573,6 +581,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());
@@ -585,6 +594,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     break;
   }
 
+  if(fabs(v_max == 0.0))
+  {
+    drive_cmd.x = 0.0;
+    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
+    return result;
+  }
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -610,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;
@@ -629,9 +644,8 @@ 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);
-
   // 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 +654,20 @@ 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);
           if(sign_x < 0.0)
-          {
+          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
-            heading_ref = angles::normalize_angle(M_PI + heading_ref);
-          }
           break;
         }
       }
 
-      const double error = angles::normalize_angle(heading_ref);
+      const double error = heading_ref;
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -664,10 +680,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
       w_target = velocity.theta + dw_heading;
+      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
     else
     {
-      w_target = 0.0;
+      w_target = std::clamp(w_target, -0.001, 0.001);
       near_goal_heading_was_active_ = false;
     }
   }
@@ -684,7 +701,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;
 
@@ -703,7 +719,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
   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_);
+    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);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -727,8 +744,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
     double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
     target_speed = max_speed * cosine_factor;
   }
-
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  double reduce_speed = std::min(max_speed, min_speed_xy_);
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
+  double reduce_speed = std::min(max_speed, v_min);
   if (s < S_final)
   {
     double r = std::clamp(s / S_final, 0.0, 1.0);
@@ -761,21 +779,29 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   // const double max_kappa = calculateMaxKappa(global_plan);
   // const bool curvature = max_kappa > straight_threshold;
   double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 2)
+  if(is_stopped && global_plan.poses.size() >= 4 &&
+  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
   {
-    const auto& p1 = global_plan.poses[1];
+    const auto& p1 = global_plan.poses[2];
-    for(int i = 2; i < global_plan.poses.size(); i++)
+    for(int i = 3; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
-      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = p.pose.x - p1.pose.x;
+      const auto& dy = p.pose.y - p1.pose.y;
+      if(std::hypot(dx, dy) > 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
+  // if(sign_x < 0.0)
+  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  // angle_to_path = path_angle;
   angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : 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)
@@ -789,16 +815,13 @@ 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
+#ifdef BUILD_WITH_ROS
-  // if (result)
+  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);
+    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
-  // else if(fabs(velocity.x) < min_speed_xy_)
+  if (result)
-  // {
+    robot::log_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);
-  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
+#endif
-  //   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
   return result;
 }
 
@@ -880,13 +903,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));
+        heading_error = angles::normalize_angle(std::atan2(dy, dx));
-        if(sign_x < 0.0)
-        {
-          heading_error = angles::normalize_angle(M_PI + heading_error);
-        }
         break;
       }
     }
@@ -936,8 +957,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   // --- Linear velocity calculation ---
   // Base velocity proportional to distance, with minimum for smooth motion
   double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   v_base = std::max(v_base, final_heading_min_velocity_);
-  v_base = std::min(v_base, min_speed_xy_);
+  v_base = std::min(v_base, v_min);
 
   // Scale down when heading error is large (prioritize rotation)
   double heading_scale = 1.0;
@@ -1009,7 +1032,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   cmd_vel.theta = omega_current + domega;
 
   // --- Apply velocity limits ---
-  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
+  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
   cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
 
   // --- Safety: ensure we can stop ---
@@ -1191,9 +1214,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   double v_limit = std::fabs(v_target);
   double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
   
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   if (journey_distance < min_journey_squared_)
   {
-    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
   }
 
   if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
@@ -1203,7 +1228,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   }
 
   if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
-    v_limit = min_speed_xy_ * sign_x;
+    v_limit = v_min * sign_x;
 
   if (fabs(decel_lim_x_) > 1e-6)
   {
@@ -1221,7 +1246,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   const robot_nav_2d_msgs::Twist2D &drive_target,
   const robot_nav_2d_msgs::Twist2D &velocity,
   const double &sign_x,
-  robot_nav_2d_msgs::Twist2D &drive_cmd)
+  robot_nav_2d_msgs::Twist2D &drive_cmd, 
+  const double &dt)
 {
   if (path.poses.empty())
   {
@@ -1229,24 +1255,31 @@ 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);
+  // nếu đường thẳng
-  drive_cmd.theta = max_vel_theta_;
+  if (max_kappa <= straight_threshold) 
-
-  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.back().pose.x) < min_lookahead_dist_ * 0.8)
     {
+      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
+        drive_cmd.theta = 0.01;
       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_)
+    const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
+    const double v_min = std::min(fabs(v_limited), min_speed_xy_);
-    return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
+    if(fabs(drive_cmd.x) < v_min) 
+    {
+      drive_cmd.x = std::copysign(v_min, sign_x);
+    }
+    return generateHermiteQuadraticTrajectory(path, sign_x);
   }
 }
 
@@ -1276,40 +1309,47 @@ 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].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
-    parallel_path.poses[i].header = path.poses[i].header;
   }
-
   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 = path.header;
-  hermite_trajectory.header.frame_id = pose.header.frame_id;
 
-  const double x = pose.pose.x;
+  if (path.poses.empty())
-  const double y = pose.pose.y;
+    return hermite_trajectory;
-  const double theta = pose.pose.theta;
+
+  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.x = x;
-    pose_stamped.pose.y = 0.0;
+    pose_stamped.pose.y = y;
-    pose_stamped.pose.theta = 0.0;
+    pose_stamped.pose.theta = theta;
-    pose_stamped.header.stamp = pose.header.stamp;
+    pose_stamped.header.stamp = hermite_trajectory.header.stamp;
-    pose_stamped.header.frame_id = pose.header.frame_id;
+    pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
     hermite_trajectory.poses.push_back(pose_stamped);
     return hermite_trajectory;
   }
@@ -1343,30 +1383,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;
+    robot_nav_2d_msgs::Pose2DStamped pose_out;
-    pose.pose.x = px;
+    pose_out.pose.x = px;
-    pose.pose.y = py;
+    pose_out.pose.y = py;
-    pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
+    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
-    pose.header.stamp = hermite_trajectory.header.stamp;
+    pose_out.header.stamp = hermite_trajectory.header.stamp;
-    pose.header.frame_id = hermite_trajectory.header.frame_id;
+    pose_out.header.frame_id = hermite_trajectory.header.frame_id;
-    hermite_trajectory.poses.push_back(pose);
+    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 = path.header;
-  trajectory.header.frame_id = pose.header.frame_id;
+  if (path.poses.empty())
+    return trajectory;
 
-  const double x = pose.pose.x;
+  const auto &goal = path.poses.back();
-  const double y = pose.pose.y;
+  if (trajectory.header.frame_id.empty())
-  const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
+    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 +1455,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__, "%.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__, "%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 %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
-        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }

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

@@ -174,8 +174,11 @@ namespace two_points_planner
     // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
     const double dx = goal.pose.position.x - start.pose.position.x;
     const double dy = goal.pose.position.y - start.pose.position.y;
-    double distance = std::sqrt(dx * dx + dy * dy);
+    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);
@@ -187,13 +190,13 @@ namespace two_points_planner
     }
     else 
     { 
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
+      robot_geometry_msgs::PoseStamped pose = start;
-      return false;
+      pose.pose.position.x += resolution * cos(theta);
+      pose.pose.position.y += resolution * sin(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_local_planner.h

@@ -52,6 +52,12 @@ namespace pnkx_local_planner
      */
     void getPlan(robot_nav_2d_msgs::Path2D &path) override;
 
+    /**
+     * @brief robot_nav_core2 getGlobalPlan - Gets the current global plan
+     * @param path The global plan
+     */
+    void getGlobalPlan(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

@@ -250,16 +250,12 @@ 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_);
+
+  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();
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -326,8 +322,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
       nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
       nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
 
-      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -454,7 +450,6 @@ 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);
     }
   }
 
@@ -560,7 +555,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
         {
           if(!dkpl_.empty()) 
           {
-            if(dkpl_.front()) delete(dkpl_.front());
+            // if(dkpl_.front()) delete(dkpl_.front());
             dkpl_.erase(dkpl_.begin());
           }
           start_docking_ = false;
@@ -800,7 +795,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/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -235,6 +235,15 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
 
 }
 
+void pnkx_local_planner::PNKXLocalPlanner::getGlobalPlan(robot_nav_2d_msgs::Path2D &path)
+{
+  if (global_plan_.poses.empty())
+  {
+    return;
+  }
+  path = global_plan_;
+}
+
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   this->getParams(planner_nh_);

src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h

@@ -89,7 +89,7 @@ public:
 
 private:
   /**
-   * @brief Resolve library path (handle relative paths, search in search_paths)
+   * @brief Resolve library path (absolute, then PNKX_NAV_CORE_LIBRARY_PATH, search_paths_, cwd)
    * @param library_path Path from config (may be relative or absolute)
    * @return Resolved absolute path, or empty if not found
    */

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -16,6 +16,70 @@
 #endif
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 
+namespace
+{
+
+  static YAML::Node xmlRpcToYaml(robot_xmlrpcpp::XmlRpcValue v)
+  {
+    using robot_xmlrpcpp::XmlRpcValue;
+    switch (v.getType())
+    {
+    case XmlRpcValue::TypeBoolean:
+    {
+      bool b = v;
+      return YAML::Node(b);
+    }
+    case XmlRpcValue::TypeInt:
+    {
+      int i = v;
+      return YAML::Node(i);
+    }
+    case XmlRpcValue::TypeDouble:
+    {
+      double d = v;
+      return YAML::Node(d);
+    }
+    case XmlRpcValue::TypeString:
+    {
+      std::string s = v;
+      return YAML::Node(s);
+    }
+    case XmlRpcValue::TypeArray:
+    {
+      YAML::Node seq(YAML::NodeType::Sequence);
+      for (int i = 0; i < v.size(); ++i)
+        seq.push_back(xmlRpcToYaml(v[i]));
+      return seq;
+    }
+    case XmlRpcValue::TypeStruct:
+    {
+      YAML::Node map(YAML::NodeType::Map);
+      const XmlRpcValue::ValueStruct *members = v.getStructMembers();
+      if (members)
+      {
+        for (const auto &kv : *members)
+          map[kv.first] = xmlRpcToYaml(kv.second);
+      }
+      return map;
+    }
+    default:
+      return YAML::Node();
+    }
+  }
+
+  static YAML::NodeType::value yamlNodeCategory(const YAML::Node &n)
+  {
+    if (n.IsMap())
+      return YAML::NodeType::Map;
+    if (n.IsSequence())
+      return YAML::NodeType::Sequence;
+    if (n.IsScalar())
+      return YAML::NodeType::Scalar;
+    return YAML::NodeType::Null;
+  }
+
+} // namespace
+
 namespace robot
 {
 
@@ -1147,92 +1211,14 @@ namespace robot
 
   void NodeHandle::setParam(const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const
   {
-    // Convert XmlRpcValue to YAML::Node
-    // Create non-const copy to use conversion operators
-    robot_xmlrpcpp::XmlRpcValue v_copy = v;
-    YAML::Node node;
     try
     {
-      switch (v.getType())
+      robot_xmlrpcpp::XmlRpcValue v_copy = v;
-      {
+      YAML::Node node = xmlRpcToYaml(v_copy);
-      case robot_xmlrpcpp::XmlRpcValue::TypeBoolean:
+      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
-      {
-        bool b = v_copy;
-        node = YAML::Node(b);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeInt:
-      {
-        int i = v_copy;
-        node = YAML::Node(i);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeDouble:
-      {
-        double d = v_copy;
-        node = YAML::Node(d);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeString:
-      {
-        std::string s = v_copy;
-        node = YAML::Node(s);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeArray:
-        {
-        YAML::Node seq(YAML::NodeType::Sequence);
-        for (int i = 0; i < v_copy.size(); ++i)
-        {
-          YAML::Node item;
-          robot_xmlrpcpp::XmlRpcValue item_v = v_copy[i];
-          if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeBoolean)
-          {
-            bool b = item_v;
-            item = YAML::Node(b);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt)
-          {
-            int i_val = item_v;
-            item = YAML::Node(i_val);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble)
-          {
-            double d = item_v;
-            item = YAML::Node(d);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString)
-          {
-            std::string s = item_v;
-            item = YAML::Node(s);
-          }
-          seq.push_back(item);
-        }
-        const_cast<NodeHandle *>(this)->setParamInternal(key, seq, YAML::NodeType::Sequence);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeStruct:
-      {
-        YAML::Node map_node(YAML::NodeType::Map);
-        // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach
-        // We'll need to iterate through the struct differently
-        // For now, create empty map
-        const_cast<NodeHandle *>(this)->setParamInternal(key, map_node, YAML::NodeType::Map);
-      }
-      break;
-      default:
-        // Unknown type, create empty node
-        const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
-        break;
-      }
     }
     catch (...)
     {
-      // On error, create empty node
       const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
     }
   }

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -98,7 +98,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
       }
     }
   }
-
   // Try LD_LIBRARY_PATH as fallback
   const char* ld_path = std::getenv("LD_LIBRARY_PATH");
   if (ld_path) {
@@ -199,16 +198,21 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
     return "";
   }
 
-  // If relative path, search in search_paths (build directory is already added)
+  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
-  std::string build_dir = getBuildDirectory();
+  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
-  if (!build_dir.empty()) {
+  if (nav_lib_path_env) {
-    // First try in build directory
-    // Add .so extension if not present
     std::string lib_path_with_ext = library_path;
     if (lib_path_with_ext.find(".so") == std::string::npos) {
       lib_path_with_ext += ".so";
     }
-    std::filesystem::path full_path = std::filesystem::path(build_dir) / lib_path_with_ext;
+    std::string nav_lib_paths(nav_lib_path_env);
+    std::stringstream ss(nav_lib_paths);
+    std::string base_dir;
+    while (std::getline(ss, base_dir, ':')) {
+      if (base_dir.empty()) {
+        continue;
+      }
+      std::filesystem::path full_path = std::filesystem::path(base_dir) / lib_path_with_ext;
       if (std::filesystem::exists(full_path)) {
         try {
           return std::filesystem::canonical(full_path).string();
@@ -217,6 +221,7 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
         }
       }
     }
+  }
 
   // Try in search_paths
   for (const auto& base_path : search_paths_) {

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

@@ -141,6 +141,13 @@ namespace robot_nav_core
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
 
+
+    /**
+     * @brief Gets the current global plan
+     * @param path The global plan
+     */
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+
     /**
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner

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

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

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h

@@ -167,6 +167,12 @@ namespace robot_nav_core_adapter
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
 
+    /**
+     * @brief Gets the current global plan
+     * @param path The global plan
+     */
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
+
     /**
      * @brief Create a new LocalPlannerAdapter
      * @return A shared pointer to the new LocalPlannerAdapter

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -407,6 +407,17 @@ namespace robot_nav_core_adapter
     path = robot_nav_2d_utils::pathToPath(path2d).poses;
   }
 
+  void LocalPlannerAdapter::getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
+  {
+    if (!planner_)
+    {
+      return;
+    }
+    robot_nav_2d_msgs::Path2D path2d;
+    planner_->getGlobalPlan(path2d);
+    path = robot_nav_2d_utils::pathToPath(path2d).poses;
+  }
+
   bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||

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

@@ -271,6 +271,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(global_planner);
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -314,6 +315,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(local_planner);
+      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -401,7 +403,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(base_global_planner);
-
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
       auto new_planner = new_loader();
@@ -1422,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)
@@ -2005,6 +2006,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         std::string behavior_name = behavior["name"].as<std::string>();
         robot::PluginLoaderHelper loader;
         std::string path_file_so = loader.findLibraryPath(behavior_type);
+        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
         // Load the factory function from the shared library
         recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2864,7 +2866,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         robot::log_debug("Goal reached!");
         resetState();
-        swapPlanner(default_config_.base_global_planner);
+        // swapPlanner(default_config_.base_global_planner);
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -3070,7 +3072,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
       resetState();
-      swapPlanner(default_config_.base_global_planner);
+      // swapPlanner(default_config_.base_global_planner);
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -3141,15 +3143,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;
@@ -3211,6 +3204,21 @@ robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
 
 robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
 {
+  if (tc_)
+  {
+    robot_nav_msgs::Path path;
+    tc_->getGlobalPlan(path.poses);
+    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)
+    {
+      robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
+      pose.header.stamp = robot::Time::now();
+      global_path.poses.push_back(goalToGlobalFrame(pose));
+    }
+    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);
   convert_data.updateFootprint(global_data_.footprint);