update bug crash miss docking

.gitignore

@@ -422,3 +422,4 @@ build
 install
 devel
 
+obstacle

CMakeLists.txt

@@ -138,6 +138,22 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
+endif()
+
+if (NOT TARGET grid_map_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
+endif()
+
+if (NOT TARGET robot_base_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
+endif()
+
+if (NOT TARGET hybrid_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
+endif()
+
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/costmap_common_params.yaml

@@ -1,10 +1,3 @@
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-base_local_planner: LocalPlannerAdapter
-base_global_planner: CustomPlanner
-
 robot_base_frame: base_link
 transform_tolerance: 1.0
 obstacle_range: 3.0
@@ -26,17 +19,33 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
-  f_scan_marking:
-    topic: /f_scan
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
+  l_scan_marking:
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
     inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
-    topic: /f_scan
+  l_scan_clearing:
+    topic: /l_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
     data_type: LaserScan
     clearing: true
     marking: false
@@ -61,3 +70,4 @@ obstacles:
     max_obstacle_height: 0.25
   
 
+

config/costmap_global_params.yaml

@@ -4,7 +4,7 @@ global_costmap:
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_local_params.yaml

@@ -5,7 +5,7 @@ local_costmap:
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/dock_global_params.yaml

@@ -0,0 +1,11 @@
+DockPlanner:
+  library_path: libdock_planner
+  MyGlobalPlanner:
+    cost_threshold: 200        # Ngưỡng chi phí vật cản (0-255)
+    safety_distance: 2         # Khoảng cách an toàn (cells)
+    use_dijkstra: false        # Sử dụng Dijkstra thay vì A*
+    
+  # File: config/costmap_params.yaml
+  global_costmap:
+    inflation_radius: 0.3      # Bán kính phình vật cản
+    cost_scaling_factor: 10.0  # Hệ số tỷ lệ chi phí

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,59 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017  
+  xy_goal_tolerance: 0.03 
+  min_approach_linear_velocity: 0.06
+
+HybridLocalPlanner:
+# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
+# HybridLocalPlanner:
+    library_path: libhybrid_local_planner
+    odom_topic: odom
+    # Trajectory
+    max_global_plan_lookahead_dist: 4.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_prune_distance: 0.0
+    global_plan_goal_sep: 0.05
+
+    # Robot
+
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.4
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.4
+    robot_max_v_backwards_pt: -0.2
+    acc_lim_x: 1.0
+    acc_lim_theta: 2.0
+    turn_around_priority: True
+    stop_dist: 0.5
+    dec_dist: 1.0
+
+
+    # GoalTolerance
+
+    xy_goal_tolerance: 0.05
+    yaw_goal_tolerance: 0.05
+
+    # Optimization
+
+    # PP Parameters
+    w_vel: 0.8
+    w_omega: 1.0
+    # DWA Parameters
+    enable_backward_motion: false
+    w_targetheading_ac: 1.7
+    w_velocity_ac: 0.2
+    w_clearance_ac: 0.2
+    w_pathDistance_ac: 0.05
+    w_smoothness_ac: 0.3
+    w_targetheading_pt: 0.2
+    w_velocity_pt: 0.8
+    w_clearance_pt: 0.1
+    w_pathDistance_pt: 2.1
+    w_smoothness_pt: 0.3
+    time_horizon: 3.0
+    velocity_resolution: 0.015
+
+    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
+    calibration_factor: 1.5 # Hệ số hiệu chuẩn
+    use_obstacle_avoidance: true # Bật tắt tránh vật cản

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,24 +30,24 @@ 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:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: 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

@@ -1,3 +1,25 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PNKXLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
 controller_frequency: 30.0   # run controller at 15.0 Hz

config/pnkx_local_planner_params.yaml

@@ -41,14 +41,14 @@ PNKXRotateLocalPlanner:
     
 LimitedAccelGenerator:
   library_path: libmkt_plugins_standard_traj_generator
-  max_vel_x: 1.0
-  min_vel_x: -1.0
+  max_vel_x: 1.2
+  min_vel_x: -1.2
 
   max_vel_y: 0.0  # diff drive robot
   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
@@ -79,6 +79,14 @@ MKTAlgorithmDiffPredictiveTrajectory:
   index_samples: 60
   follow_step_path: true
 
+  # Kalman filter tuning (filters v and w commands)
+  kf_q_v: 0.25
+  kf_q_w: 0.8
+  kf_r_v: 0.05
+  kf_r_w: 0.08
+  kf_p0: 0.5
+  kf_filter_angular: false
+
   # Lookahead
   use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
   # only when false:
@@ -98,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
@@ -135,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
@@ -172,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

examples/NavigationExample/NavigationAPI.cs

@@ -9,7 +9,7 @@ namespace NavigationExample
     /// </summary>
     public class NavigationAPI
     {
-        private const string DllName = "libnav_c_api.so"; // Linux
+        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
         // For Windows: "nav_c_api.dll"
         // For macOS: "libnav_c_api.dylib"
 
@@ -152,6 +152,10 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);

examples/NavigationExample/Program.cs

@@ -396,8 +396,8 @@ namespace NavigationExample
 
             PoseStamped goal = new PoseStamped();
             goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
-            goal.pose.position.x = 1.0;
-            goal.pose.position.y = 1.0;
+            goal.pose.position.x = 0.01;
+            goal.pose.position.y = 0.01;
             goal.pose.position.z = 0.0;
             goal.pose.orientation.x = 0.0;
             goal.pose.orientation.y = 0.0;
@@ -406,9 +406,9 @@ namespace NavigationExample
 
 
             // Console.WriteLine("Docking to docking_point");
-            // NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
+            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);
 
-            NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
+            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
 
             // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
             NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);

examples/run_example.sh

@@ -27,6 +27,7 @@ echo "Building C API library..."
 cd "$BUILD_DIR"
 cmake ..
 make 
+sudo make install
 echo "Library built successfully!"
 
 
@@ -83,9 +84,9 @@ fi
 # Luôn copy source code mới nhất (cập nhật file nếu đã có)
 cd "$EXAMPLE_DIR/NavigationExample"
 
-# Bước 3: Copy library
-echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# # Bước 3: Copy library
+# echo "Copying library..."
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 # Main build directory (contains libtf3.so, librobot_cpp.so, etc.)

src/APIs/c_api/include/nav_c_api.h

@@ -171,6 +171,20 @@ extern "C"
    */
   bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
 
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+   bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
+
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle

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();
@@ -397,6 +398,44 @@ extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle || !marker)
+  {
+    return false;
+  }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
+  try
+  {
+    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
+        std::shared_ptr<robot::move_base_core::BaseNavigation>(
+            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
+    if (!nav_ptr)
+      return false;
+    Order order;
+    order.nodes = const_cast<Node *>(nodes);
+    order.nodes_count = node_count;
+    order.edges = const_cast<Edge *>(edges);
+    order.edges_count = edge_count;
+    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
+    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
+    return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to dock to nodes edges\n", __FILE__, __LINE__);
+    return false;
+  }
+}
+
 extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
 {
   if (!handle)

src/Algorithms/Libraries/kalman/src/kalman.cpp

@@ -55,11 +55,34 @@ void KalmanFilter::update(const Eigen::VectorXd& y) {
   if(!initialized)
     throw std::runtime_error("Filter is not initialized!");
 
+  if (y.size() != m)
+    throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size");
+
+  if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n ||
+      Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m ||
+      P.rows() != n || P.cols() != n)
+    throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent");
+
   x_hat_new = A * x_hat;
   P = A*P*A.transpose() + Q;
-  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
+
+  const Eigen::MatrixXd S = C * P * C.transpose() + R;
+  Eigen::LDLT<Eigen::MatrixXd> ldlt(S);
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance");
+
+  // K = P*C' * inv(S)  -> solve(S * X = (P*C')^T)
+  const Eigen::MatrixXd PCt = P * C.transpose();
+  const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose());
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain");
+  K = KT.transpose();
+
   x_hat_new += K * (y - C*x_hat_new);
-  P = (I - K*C)*P;
+
+  // Joseph form: keeps P symmetric / PSD under numeric errors
+  const Eigen::MatrixXd IKC = I - K * C;
+  P = IKC * P * IKC.transpose() + K * R * K.transpose();
   x_hat = x_hat_new;
 
   t += dt;

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
@@ -412,6 +414,14 @@ namespace mkt_algorithm
       Eigen::MatrixXd Q;
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
+
+      // Kalman filter tuning (for v and w filtering)
+      double kf_q_v_;
+      double kf_q_w_;
+      double kf_r_v_;
+      double kf_r_w_;
+      double kf_p0_;
+      bool kf_filter_angular_;
 #ifdef BUILD_WITH_ROS
       ros::Publisher lookahead_point_pub_;
 #endif

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

@@ -35,6 +35,12 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
     cost_scaling_gain_(0.0),
     control_duration_(0.0),
     kf_(nullptr),
+    kf_q_v_(0.25),
+    kf_q_w_(0.8),
+    kf_r_v_(0.05),
+    kf_r_w_(0.08),
+    kf_p0_(0.5),
+    kf_filter_angular_(false),
     m_(0),
     n_(0)
 {
@@ -83,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     // kalman
     last_actuator_update_ = robot::Time::now();
-    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
-    m_ = 2; // measurements: x, y, theta
+    // State: [v, a, j, w, alpha, jw] where:
+    //  - v: linear velocity, a: linear accel, j: linear jerk
+    //  - w: angular velocity, alpha: angular accel, jw: angular jerk
+    // Measurement: [v, w]
+    n_ = 6;
+    m_ = 2;
     double dt = control_duration_;
 
     // Khởi tạo ma trận
     A = Eigen::MatrixXd::Identity(n_, n_);
     C = Eigen::MatrixXd::Zero(m_, n_);
     Q = Eigen::MatrixXd::Zero(n_, n_);
-    R = Eigen::MatrixXd::Identity(m_, m_);
-    P = Eigen::MatrixXd::Identity(n_, n_);
+    R = Eigen::MatrixXd::Zero(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
 
     for (int i = 0; i < n_; i += 3)
     {
@@ -103,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     C(0, 0) = 1;
     C(1, 3) = 1;
-    Q(2, 2) = 0.1;
-    Q(5, 5) = 0.6;
+    Q(2, 2) = std::max(1e-12, kf_q_v_);
+    Q(5, 5) = std::max(1e-12, kf_q_w_);
 
-    R(0, 0) = 0.1;
-    R(1, 1) = 0.2;
-
-    P(3, 3) = 0.4;
-    P(4, 4) = 0.4;
-    P(5, 5) = 0.4;
+    R(0, 0) = std::max(1e-12, kf_r_v_);
+    R(1, 1) = std::max(1e-12, kf_r_w_);
 
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
@@ -177,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
+
+  // Kalman filter tuning (filtering v and w commands)
+  nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
+  nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
+  nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
+  nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
+  nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
+  nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);
+
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
   control_duration_ = 1.0 / control_frequency;
 
@@ -264,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__);
@@ -273,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)
   {
@@ -296,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;
@@ -314,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;
@@ -345,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();
@@ -353,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)
   {
@@ -363,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--)
@@ -391,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;
@@ -407,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)
     {
@@ -427,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;
@@ -461,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())
   {
@@ -497,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))
   {
@@ -513,24 +543,20 @@ 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);
       
@@ -544,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_)
     {
@@ -558,25 +584,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
-
-    // Eigen::VectorXd y(2);
-    // y << drive_cmd.x, drive_cmd.theta;
-
-    // // Cập nhật lại A nếu dt thay đổi
-    // for (int i = 0; i < n_; ++i)
-    //   for (int j = 0; j < n_; ++j)
-    //     A(i, j) = (i == j ? 1.0 : 0.0);
-    // for (int i = 0; i < n_; i += 3)
-    // {
-    //   A(i, i + 1) = dt;
-    //   A(i, i + 2) = 0.5 * dt * dt;
-    //   A(i + 1, i + 2) = dt;
-    // }
-    // kf_->update(y, dt, A);
-    // double v_min = min_approach_linear_velocity_;
-    // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
-    // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
   }
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
@@ -589,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;
@@ -606,7 +615,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 {
   // 1) Curvature from pure pursuit
   const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  const double L2_min = 0.05;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_min = 0.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
   const double L2_safe = std::max(L2, L2_min);
   const double L = std::sqrt(L2_safe);
 
@@ -634,6 +643,8 @@ 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;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -644,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_, 
@@ -667,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;
     }
   }
@@ -679,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)
@@ -687,6 +704,27 @@ 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;
+
+  // Cập nhật lại A nếu dt thay đổi
+  for (int i = 0; i < n_; ++i)
+    for (int j = 0; j < n_; ++j)
+      A(i, j) = (i == j ? 1.0 : 0.0);
+  for (int i = 0; i < n_; i += 3)
+  {
+    A(i, i + 1) = dt;
+    A(i, i + 2) = 0.5 * dt * dt;
+    A(i + 1, i + 2) = dt;
+  }
+  kf_->update(y, dt, A);
+  double v_min = min_approach_linear_velocity_;
+  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_);
+  // robot::log_info("%s", ss.str().c_str());
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -750,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_;
@@ -863,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;
       }
     }
@@ -1044,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.");
@@ -1081,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())
@@ -1219,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);
   }
 }
 
@@ -1259,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;
   }
@@ -1326,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)
   {
@@ -1389,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

@@ -36,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -121,6 +119,7 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    
     robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
     robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -148,21 +147,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    unsigned int mx_start, my_start;
-    unsigned int mx_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
-      mx_start, my_start)
+    // unsigned int mx_start, my_start;
+    // unsigned int mx_end, my_end;
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    //   start.pose.position.y,
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
-        mx_end, my_end))
-    {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
-    }
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //     goal.pose.position.y,
+    //     mx_end, my_end))
+    // {
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   return false;
+    // }
     // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
@@ -175,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);
@@ -188,13 +190,18 @@ namespace two_points_planner
     }
     else 
     { 
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+      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/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

@@ -74,7 +74,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -83,7 +83,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name; 
@@ -99,14 +99,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -121,7 +121,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -129,7 +129,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -153,7 +153,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();
@@ -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.");
@@ -252,13 +258,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 
   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);
+
   parent_.setParam(algorithm_nav_name, original_papams_);
-  nh_algorithm.setParam("allow_rotate", false);
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+  // 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_)
   {
@@ -272,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_;
@@ -284,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");
   }
@@ -293,7 +310,10 @@ 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())
+  {
+    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 4.0");
+    if(nav_algorithm_)
     {
       if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
       {
@@ -302,9 +322,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
       }
       // 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);
@@ -321,9 +342,9 @@ 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_);
-      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
-
+        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_)
         {
 
@@ -341,6 +362,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
             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");
+      }
     }
   }
   else
@@ -364,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)
@@ -393,7 +429,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
   else if (!ret_nav_)
   {
     traj = nav_algorithm_->calculator(pose, velocity);
-
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
@@ -401,9 +436,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
         traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
       }
     }
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -411,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!");
@@ -442,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));
@@ -503,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)
@@ -527,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)
@@ -535,6 +585,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             }
           }
         }
+        
       }
       else
       {
@@ -548,7 +599,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;
@@ -567,17 +618,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
-    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+    : initialized_(false),
+      is_detected_(false),
+      is_goal_reached_(false),
+      following_(false),
+      allow_rotate_(false),
+      xy_goal_tolerance_(0.05),
+      yaw_goal_tolerance_(0.05),
+      min_lookahead_dist_(0.4),
+      max_lookahead_dist_(1.0),
+      lookahead_time_(1.5),
+      angle_threshold_(0.4),
+      name_(name),
+      docking_planner_name_(),
+      docking_nav_name_(),
+      docking_planner_(nullptr),
+      docking_nav_(nullptr),
+      linear_(),
+      angular_(),
+      nh_(),
+      nh_priv_(),
+      tf_(nullptr),
+      costmap_robot_(nullptr),
+      traj_generator_(),
+      docking_planner_loader_(),
+      docking_nav_loader_(),
+      detected_timeout_wt_(),
+      delayed_wt_(),
+      delayed_(false),
+      detected_timeout_(false),
+      robot_base_frame_(),
+      maker_goal_frame_()
 {
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  if (docking_planner_)
+  detected_timeout_wt_.stop();
+  delayed_wt_.stop();
+  if (docking_planner_ != nullptr) {
     docking_planner_.reset();
-  if (docking_nav_)
+  }
+  if (docking_nav_ != nullptr) {
     docking_nav_.reset();
+  }
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
@@ -593,14 +677,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
       docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
         path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
       docking_planner_ = docking_planner_loader_();
       if (!docking_planner_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
       }
       docking_planner_->initialize(name_, costmap_robot_);
       robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -616,14 +701,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
       docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
       docking_nav_ = docking_nav_loader_();
       if (!docking_nav_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
       }
       robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
       docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
@@ -753,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/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -80,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
@@ -104,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -126,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -134,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());

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

@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -97,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
     
     std::string algorithm_nav_name; 
@@ -107,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -135,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -143,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -159,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -167,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();
@@ -234,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/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -64,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -72,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
       nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
@@ -96,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -111,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -119,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
     
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());

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
 {
 
@@ -651,7 +715,8 @@ namespace robot
           std::string key = it->first.as<std::string>();
           const YAML::Node &value = it->second;
           std::string full_key = prefix.empty() ? key : prefix + "/" + key;
-
+          if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
+            continue;
           if (value.IsMap())
           {
             std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;
@@ -1146,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())
-      {
-      case robot_xmlrpcpp::XmlRpcValue::TypeBoolean:
-      {
-        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;
-      }
+      robot_xmlrpcpp::XmlRpcValue v_copy = v;
+      YAML::Node node = xmlRpcToYaml(v_copy);
+      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
     }
     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

@@ -97,7 +97,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) {
@@ -198,16 +197,21 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
     return "";
   }
 
-  // If relative path, search in search_paths (build directory is already added)
-  std::string build_dir = getBuildDirectory();
-  if (!build_dir.empty()) {
-    // First try in build directory
-    // Add .so extension if not present
+  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
+  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
+  if (nav_lib_path_env) {
     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();
@@ -216,6 +220,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

@@ -41,6 +41,7 @@
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
+#include <robot_nav_msgs/Odometry.h>
 #include <memory>
 
 namespace robot_nav_core
@@ -140,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) {return;}
+
     /**
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner
@@ -148,6 +156,12 @@ namespace robot_nav_core
      */
     virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 
+    /**
+     * @brief Set the odometry of the robot
+     * @param odom The odometry of the robot
+     */
+    virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
+
     /**
      * @brief  Virtual destructor for the interface
      */
@@ -155,6 +169,11 @@ namespace robot_nav_core
 
   protected:
     BaseLocalPlanner() {}
+
+    /**
+     * @brief The odometry of the robot
+     */
+     robot_nav_msgs::Odometry *odom_;
   };
 } // namespace robot_nav_core
 

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) {return;}
+
     /**
      * @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
@@ -189,11 +195,6 @@ namespace robot_nav_core_adapter
     robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
-    /**
-     * @brief The odometry of the robot
-     */
-    robot_nav_msgs::Odometry odom_;
-
     // Plugin handling
     boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
     robot_nav_core2::LocalPlanner::Ptr planner_;

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

@@ -291,7 +291,7 @@ namespace robot_nav_core_adapter
 
   robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
-    return odom_.twist.twist;
+    return odom_->twist.twist;
   }
 
   bool LocalPlannerAdapter::islock()
@@ -356,7 +356,7 @@ namespace robot_nav_core_adapter
     if (!getRobotPose(pose2d))
       return false;
 
-    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
+    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {
@@ -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

@@ -31,7 +31,9 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
-#include <laser_filter/laser_filter.h>
+#ifndef BUILD_WITH_ROS
+  #include <laser_filter/laser_filter.h>
+#endif
 
 move_base::MoveBase::MoveBase()
   : initialized_(false),
@@ -269,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);
 
@@ -312,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);
@@ -322,6 +326,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
         throw std::runtime_error("Failed to load local planner " + local_planner);
       }
       tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
+      tc_->setOdom(&odometry_);
     }
     catch (const std::exception &ex)
     {
@@ -398,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();
@@ -500,6 +505,7 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
 void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
 {
   auto it = laser_scans_.find(laser_scan_name);
+#ifndef BUILD_WITH_ROS
   laser_filter::LaserScanSOR sor;
   sor.setMeanK(10);              // xét 10 điểm láng giềng gần nhất
   sor.setStddevMulThresh(1.0);   // ngưỡng = mean + 1.0 * stddev
@@ -512,6 +518,16 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   {
     it->second = laser_scan_filter;
   }
+#else
+  if (it == laser_scans_.end())
+  {
+    laser_scans_[laser_scan_name] = laser_scan;
+  }
+  else
+  {
+    it->second = laser_scan;
+  }
+#endif
   // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
   // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
   // robot::log_info("angle_min: %f", laser_scan.angle_min);
@@ -534,8 +550,13 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // }
   // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
+#ifndef BUILD_WITH_ROS
   updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
   updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#else
+  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+#endif
 }
 
 robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
@@ -811,9 +832,19 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
                                   double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -945,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)
@@ -962,9 +994,19 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1075,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)
@@ -1119,9 +1160,19 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
   }
 
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1195,6 +1246,47 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
   }
   if (cancel_ctr_)
     cancel_ctr_ = false;
+  // Check if action server exists
+  if (!as_)
+  {
+    robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
+    lock.unlock();
+    return false;
+  }
+
+  try
+  {
+    robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
+    action_goal->header.stamp = robot::Time::now();
+    action_goal->goal.target_pose = goal;
+    
+    // Generate unique goal ID using timestamp
+    robot::Time now = robot::Time::now();
+    action_goal->goal_id.stamp = now;
+    std::ostringstream goal_id_stream;
+    goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
+    action_goal->goal_id.id = goal_id_stream.str();
+    
+    robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+    robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", 
+                    action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
+    
+    // Clear Order message since this is a non-Order moveTo call
+    {
+      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
+      planner_order_.reset();
+    }
+    
+    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)
+  {
+    lock.unlock();
+    robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
+    return false;
+  }
 
   lock.unlock();
   return true;
@@ -1232,9 +1324,19 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
   }
 
   if (setup_)
+  {
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
     {
       swapPlanner(default_config_.base_global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1322,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)
@@ -1342,8 +1443,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, go_straight_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
       swapPlanner(global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1465,8 +1576,18 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, rotate_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
       swapPlanner(global_planner);
     }
+  }
   else
   {
     if (nav_feedback_)
@@ -1662,32 +1783,6 @@ void move_base::MoveBase::cancel()
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   cancel_ctr_ = true;
   robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
-  
-  if (as_)
-  {
-    // Get current goal ID from action server to cancel specific goal
-    // If we want to cancel all goals, use empty string ""
-    robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
-    
-    // Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
-    msg->id = "";  // Empty string cancels current goal
-    msg->stamp = robot::Time::now();
-    
-    robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
-    robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", 
-                    msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
-    
-    // Convert to ConstPtr for processCancel
-    robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
-    as_->processCancel(cancel_msg);
-    robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
-  }
-  else
-  {
-    robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
-  }
-  
-  robot::log_info("[MoveBase::cancel] ===== EXIT =====");
 }
 
 bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -1911,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);
@@ -2262,8 +2358,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
       robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
     }
   }
-
+  robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
   robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
+  robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
   publishZeroVelocity();
   // we have a goal so start the planner
   boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2398,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;
@@ -2483,7 +2581,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
     robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
-
   tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
 
   // next, we need to check if the length of the quaternion is close to zero
@@ -2618,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_))
     {
@@ -2639,27 +2736,44 @@ 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)
       recovery_index_ = 0;
   }
 
+
   // Cancle executeCycle
   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
     {
-      if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+      double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+      if (actual_linear_velocity <= min_approach_linear_velocity_)
       {
         robot::log_info("MoveTo is canceled.");
         resetState();
+        cancel_ctr_ = false;
         // if(default_config_.base_global_planner != last_config_.base_global_planner)
+        robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+        if(nh_planner.hasParam("base_global_planner"))
+        {
+          std::string base_global_planner;
+          nh_planner.getParam("base_global_planner", base_global_planner);
+          swapPlanner(base_global_planner);
+        }
+        else
+        {
           swapPlanner(default_config_.base_global_planner);
-
+        }
+        cancel_ctr_ = false;
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -2670,7 +2784,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
         }
         // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
-        cancel_ctr_ = false;
+
+        if (as_)
+        {
+          robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+          msg->id = "";  // Empty string cancels current goal
+          msg->stamp = robot::Time::now();
+          // Convert to ConstPtr for processCancel
+          robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+          as_->processCancel(cancel_msg);
+          robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+        }
+        else
+        {
+          robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+        }
         return true;
       }
     }
@@ -2679,7 +2807,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       robot::log_info("MoveTo is canceled.");
       resetState();
       // if(default_config_.base_global_planner != last_config_.base_global_planner)
+      robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+      if(nh_planner.hasParam("base_global_planner"))
+      {
+        std::string base_global_planner;
+        nh_planner.getParam("base_global_planner", base_global_planner);
+        swapPlanner(base_global_planner);
+      }
+      else
+      {
         swapPlanner(default_config_.base_global_planner);
+      }
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2691,6 +2829,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
       }
       // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
+      if (as_)
+      {
+        robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+        msg->id = "";  // Empty string cancels current goal
+        msg->stamp = robot::Time::now();
+        // Convert to ConstPtr for processCancel
+        robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+        as_->processCancel(cancel_msg);
+        robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+      }
+      else
+      {
+        robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+      }
       cancel_ctr_ = false;
       return true;
     }
@@ -2711,7 +2863,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
   // if we're controlling, we'll attempt to find valid velocity commands
   case move_base::CONTROLLING:
-    // robot::log_debug("In controlling state.");
 
     // check to see if we've reached our goal
     try
@@ -2720,7 +2871,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;
@@ -2769,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())
@@ -2798,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
         {
@@ -2885,7 +3039,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         tc_->setTwistLinear(linear);
         try
         {
-          if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+          double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+          if (actual_linear_velocity <= min_approach_linear_velocity_)
           {
             paused_ = true;
           }
@@ -2925,7 +3080,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_);
@@ -2989,16 +3144,40 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
 robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
 {
-  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
+  if (!tf_)
+  {
+    return goal_pose_msg;
+  }
+
+  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
+  robot_geometry_msgs::PoseStamped global_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
+  robot_geometry_msgs::PoseStamped goal_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
+  goal_pose = goal_pose_msg;
+  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
+  tf3::Time tf3_current_time = data_convert::convertTime(current_time);
+  tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
+  
   try
   {
-    tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+    // use current time if possible (makes sure it's not in the future)
+    std::string error_msg;
+    if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
+    {
+      // Transform is available at current time
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
       tf3::doTransform(goal_pose, global_pose, transform);
     }
+    // use the latest otherwise (tf3::Time() means latest available)
+    else
+    {
+      // Try to get latest transform
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
+      tf3::doTransform(goal_pose, global_pose, transform);
+    }
+  }
   catch (tf3::LookupException &ex)
   {
     robot::log_error("Move Base No Transform available Error looking up robot pose: %s", ex.what());
@@ -3033,6 +3212,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);