add dock to

add log
add some files
2026-03-19 04:25:59 +00:00 · 2026-03-19 04:12:51 +00:00 · 2026-03-18 06:51:10 +00:00
41 changed files with 549 additions and 820 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -422,3 +422,4 @@ build
 install
 devel

+obstacle
--- a/CATKIN_BUILD_README.md
+++ b/CATKIN_BUILD_README.md
@@ -20,11 +20,7 @@ The specified base path contains a CMakeLists.txt but "catkin_make" must be invo

 # Build trong workspace mới
 cd ../pnkx_nav_catkin_ws
-rm -rf build devel
-catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo \
-  -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
-  -DCMAKE_C_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
-  -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address"
+catkin_make
 source devel/setup.bash
 ```

--- a/CMAKE_BUILD_README.md
+++ b/CMAKE_BUILD_README.md
@@ -356,7 +356,8 @@ export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config

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

--- a/CMakeLists.txt
+++ b/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()
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -1,3 +1,11 @@
+# position_planner_name: PNKXLocalPlanner
+position_planner_name: HybridLocalPlanner
+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
@@ -19,17 +27,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
@@ -54,3 +78,4 @@ obstacles:
    max_obstacle_height: 0.25
  

+
--- a/config/costmap_global_params.yaml
+++ b/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
--- a/config/costmap_local_params.yaml
+++ b/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
--- a/config/dock_global_params.yaml
+++ b/config/dock_global_params.yaml
@@ -1,11 +0,0 @@
-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í
--- a/config/hybrid_local_planner_params.yaml
+++ b/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.1
+    yaw_goal_tolerance: 0.07
+
+    # 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
--- a/config/maker_sources.yaml
+++ b/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.2
+    vel_x: 0.25
    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.4
+    angle_threshold: 0.16

  qrcode:
    maker_goal_frame: qr_trolley
@@ -30,7 +30,7 @@ trolley:
    min_lookahead_dist: 0.4
    max_lookahead_dist: 1.0
    lookahead_time: 1.5
-    angle_threshold: 0.4
+    angle_threshold: 0.16

 charger:
  plugins:
@@ -41,13 +41,13 @@ charger:
    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
    delay: 1.5 # Cấm sửa không là không chạy được
    timeout: 60
-    vel_x: 0.15
+    vel_x: 0.1
    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.4
+    angle_threshold: 0.16

 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.4
+    angle_threshold: 0.16
    
  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.4
+    angle_threshold: 0.16

 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.4
+    angle_threshold: 0.16

  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.4
+    angle_threshold: 0.16
--- a/config/move_base_common_params.yaml
+++ b/config/move_base_common_params.yaml
@@ -1,25 +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
-
-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
@@ -30,7 +8,7 @@ max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
 oscillation_distance: 0.5
 ### recovery behaviors
-recovery_behavior_enabled: false 
+recovery_behavior_enabled: true
 recovery_behaviors: [       
                      {name: aggressive_reset, type: ClearCostmapRecovery},
                      {name: conservative_reset, type: ClearCostmapRecovery},                      
--- a/config/pnkx_local_planner_params.yaml
+++ b/config/pnkx_local_planner_params.yaml
@@ -1,5 +1,5 @@
-yaw_goal_tolerance: 0.02  
-xy_goal_tolerance: 0.03 
+yaw_goal_tolerance: 0.03  
+xy_goal_tolerance: 0.02 
 min_approach_linear_velocity: 0.05 

 LocalPlannerAdapter:
@@ -41,8 +41,8 @@ PNKXRotateLocalPlanner:
    
 LimitedAccelGenerator:
  library_path: libmkt_plugins_standard_traj_generator
-  max_vel_x: 1.2
-  min_vel_x: -1.2
+  max_vel_x: 0.2
+  min_vel_x: -0.2

  max_vel_y: 0.0  # diff drive robot
  min_vel_y: 0.0  # diff drive robot
@@ -50,15 +50,15 @@ LimitedAccelGenerator:
  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

-  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
  min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

-  acc_lim_x: 3.0
+  acc_lim_x: 1.5
  acc_lim_y: 0.0      # diff drive robot
  acc_lim_theta: 1.5
-  decel_lim_x: -3.0
+  decel_lim_x: -1.5
  decel_lim_y: -0.0
-  decel_lim_theta: -2.0
+  decel_lim_theta: -1.5

  # Whether to split the path into segments or not
  split_path: true
@@ -74,19 +74,11 @@ LimitedAccelGenerator:

 MKTAlgorithmDiffPredictiveTrajectory:
  library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.05
-  angle_threshold: 0.6
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.47
  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:
@@ -106,8 +98,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
  angular_decel_zone: 0.1             

  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.03
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
@@ -119,7 +111,7 @@ MKTAlgorithmDiffPredictiveTrajectory:

 MKTAlgorithmDiffGoStraight:
  library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.05
+  xy_local_goal_tolerance: 0.02
  angle_threshold: 0.8
  index_samples: 60
  follow_step_path: true
@@ -143,8 +135,8 @@ MKTAlgorithmDiffGoStraight:
  angular_decel_zone: 0.1             

  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.03
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
@@ -156,7 +148,7 @@ MKTAlgorithmDiffGoStraight:

 MKTAlgorithmDiffRotateToGoal:
  library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.05
+  xy_local_goal_tolerance: 0.02
  angle_threshold: 0.47
  index_samples: 60
  follow_step_path: true
@@ -180,8 +172,8 @@ MKTAlgorithmDiffRotateToGoal:
  angular_decel_zone: 0.1             

  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.03
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
  
  use_final_heading_alignment: true
  final_heading_xy_tolerance: 0.1
--- a/examples/NavigationExample/NavigationAPI.cs
+++ b/examples/NavigationExample/NavigationAPI.cs
@@ -9,7 +9,7 @@ namespace NavigationExample
    /// </summary>
    public class NavigationAPI
    {
-        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
+        private const string DllName = "libnav_c_api.so"; // Linux
        // For Windows: "nav_c_api.dll"
        // For macOS: "libnav_c_api.dylib"

@@ -152,10 +152,6 @@ 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);
--- a/examples/NavigationExample/Program.cs
+++ b/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 = 0.01;
-            goal.pose.position.y = 0.01;
+            goal.pose.position.x = 1.0;
+            goal.pose.position.y = 1.0;
            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(navHandle, "charger", goal);
+            // NavigationAPI.navigation_dock_to_order(navHandle, order, "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);
--- a/examples/NavigationExample/libnav_c_api.so
+++ b/examples/NavigationExample/libnav_c_api.so
--- a/examples/run_example.sh
+++ b/examples/run_example.sh
@@ -27,7 +27,6 @@ echo "Building C API library..."
 cd "$BUILD_DIR"
 cmake ..
 make 
-sudo make install
 echo "Library built successfully!"


@@ -84,9 +83,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.)
--- a/src/APIs/c_api/src/nav_c_api.cpp
+++ b/src/APIs/c_api/src/nav_c_api.cpp
@@ -83,7 +83,6 @@ 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();
@@ -569,16 +568,10 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
      return false;

    robot_geometry_msgs::Vector3 linear;
-    linear.x = 0.1;
+    linear.x = linear_x;
    linear.y = linear_y;
    linear.z = linear_z;
-    bool result = nav_ptr->setTwistLinear(linear);
-    robot::log_info("setTwistLinear Forward %f", linear.x);
-    
-    linear.x = -0.1;
-    result &= result && nav_ptr->setTwistLinear(linear);
-    robot::log_info("setTwistLinear Backward %f", linear.x);
-    return result;
+    return nav_ptr->setTwistLinear(linear);
  }
  catch (...)
  {
--- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
+++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
@@ -206,7 +206,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
    // Process index_s with multiple elements
    if (index_s.size() > 1)
    {
-      for (size_t i = 1; i < index_s.size(); ++i)
+      for (size_t i = 0; i < index_s.size(); ++i)
      {
        if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
        {
@@ -219,12 +219,11 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
        double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
        double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;

-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
        {
          double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
          if (fabs(tolerance) <= xy_local_goal_tolerance_)
          {
-
            if (index_s[i] > sub_goal_index_saved_)
            {
              sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
@@ -258,7 +257,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
        double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
        double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
        double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
        {
          double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
          if (fabs(tolerance) <= xy_local_goal_tolerance_)
@@ -416,7 +415,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
  robot_nav_2d_msgs::Pose2DStamped sub_pose;
  sub_pose = global_plan.poses[closet_index];

-#ifdef BUILD_WITH_ROS
+#ifdef SCORE_ALGORITHM_WITH_ROS
  {
    robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
    geometry_msgs::PoseStamped sub_pose_stamped_ros;
@@ -424,7 +423,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
    sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
    sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
    sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
-    sub_pose_stamped_ros.pose.position.z = 0.9;
+    sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
    sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
    sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
    sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
@@ -435,7 +434,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs

  robot_nav_2d_msgs::Pose2DStamped sub_goal;
  sub_goal = global_plan.poses[goal_index];
-#ifdef BUILD_WITH_ROS
+#ifdef SCORE_ALGORITHM_WITH_ROS
  {
    robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
    geometry_msgs::PoseStamped sub_goal_stamped_ros;
@@ -443,7 +442,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
    sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
    sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
    sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
-    sub_goal_stamped_ros.pose.position.z = 0.9;
+    sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
    sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
    sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
    sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;
--- a/src/Algorithms/Libraries/kalman/src/kalman.cpp
+++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp
@@ -55,34 +55,11 @@ 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;
-
-  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();
-
+  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
  x_hat_new += K * (y - C*x_hat_new);
-
-  // 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();
+  P = (I - K*C)*P;
  x_hat = x_hat_new;

  t += dt;
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
@@ -181,7 +181,7 @@ namespace mkt_algorithm
       */
      robot_nav_2d_msgs::Path2D generateTrajectory(  
        const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
-        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);

      /**
       * @brief Generate trajectory
@@ -194,11 +194,11 @@ namespace mkt_algorithm

      /**
       * @brief Generate Hermite trajectory
-       * @param path
+       * @param pose
       * @param sign_x
       * @return trajectory
       */
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);

      /**
       * @brief Generate Hermite quadratic trajectory
@@ -206,7 +206,7 @@ namespace mkt_algorithm
       * @param sign_x
       * @return trajectory
       */
-      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);

      /**
       * @brief Should rotate to path
@@ -412,14 +412,6 @@ 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
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
@@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
  robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
  
-//   // === Final Heading Alignment Check ===
-//   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("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-//                       xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-// #endif
-//   }
-//   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("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+                      xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+#endif
+  }
+  else
+  {
    // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", 
    //   journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
    if(fabs(carrot_pose.pose.y) > 0.2)
@@ -151,7 +151,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
      lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
    }
    robot_nav_2d_msgs::Twist2D drive_target;
-    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
    carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);

    // Normal Pure Pursuit
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
    sign_x, 
    dt, 
    drive_cmd);
-  // }
+  }
  applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);

  if (this->nav_stop_)
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -35,12 +35,6 @@ 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)
 {
@@ -89,20 +83,16 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(

    // kalman
    last_actuator_update_ = robot::Time::now();
-    // 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;
+    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
+    m_ = 2; // measurements: x, y, theta
    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::Zero(m_, m_);
-    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
+    R = Eigen::MatrixXd::Identity(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_);

    for (int i = 0; i < n_; i += 3)
    {
@@ -113,11 +103,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(

    C(0, 0) = 1;
    C(1, 3) = 1;
-    Q(2, 2) = std::max(1e-12, kf_q_v_);
-    Q(5, 5) = std::max(1e-12, kf_q_w_);
+    Q(2, 2) = 0.1;
+    Q(5, 5) = 0.6;

-    R(0, 0) = std::max(1e-12, kf_r_v_);
-    R(1, 1) = std::max(1e-12, kf_r_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;

    kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
    Eigen::VectorXd x0(n_);
@@ -183,15 +177,6 @@ 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;

@@ -368,7 +353,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
    robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
    return false;
  }
-
  const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
  if(fabs(carrot_pose.pose.y) > 0.2)
  {
@@ -405,47 +389,42 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
    }
    
  }
-  else if(compute_plan_.poses.size() == 1)
+  else
  {
    try
    {
-      // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
-      // auto prev_carrot_pose_it = transform_plan_.poses.begin();
-      // double distance_it = 0;
-      // for (auto it = carrot_pose_it - 1; 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;
-      //   distance_it += std::hypot(dx, dy);
-      //   if (distance_it > costmap_robot_->getCostmap()->getResolution())
-      //   {
-      //     prev_carrot_pose_it = it;
-      //     break;
-      //   }
-      // }
+      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      double distance_it = 0;
+      for (auto it = carrot_pose_it - 1; 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;
+        distance_it += std::hypot(dx, dy);
+        if (distance_it > costmap_robot_->getCostmap()->getResolution())
+        {
+          prev_carrot_pose_it = it;
+          break;
+        }
+      }

-      // 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 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_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);

      // teb_local_planner::PoseSE2 start_pose(front);
      // teb_local_planner::PoseSE2 goal_pose(back);
      // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
-
-      
-      auto goal_pose = compute_plan_.poses.front().pose;
-      auto start_pose = pose.pose;
-      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
-      double dir_path = cos(fabs(angle_path - goal_pose.theta));
+      const double dir_path = 0.0;
      if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
        x_direction = dir_path > 0 ? FORWARD : BACKWARD;
    }
    catch (std::exception &e)
    {
-      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      x_direction = x_direction_; 
+      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      return false;
    }
  }

@@ -483,8 +462,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
  }
  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
-  // drive_cmd.x = sqrt(twist.x * twist.x);
-  
  robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
  if (transformed_plan.poses.empty())
  {
@@ -520,10 +497,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
  const double distance_allow_rotate = min_journey_squared_;
  const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
  allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-  // robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
-  // allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0;
-
-  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);

  double angle_to_heading;
  if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
@@ -540,21 +513,25 @@ 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
-//     {
-      robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
+    // === 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;
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
      carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);

      // Normal Pure Pursuit
@@ -567,7 +544,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_)
    {
@@ -582,6 +559,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
      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());
@@ -594,12 +589,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
    break;
  }

-  if(fabs(v_max == 0.0))
-  {
-    drive_cmd.x = 0.0;
-    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
-    return result;
-  }
  result.velocity = drive_cmd;
  prevous_drive_cmd_ = drive_cmd;
  return result;
@@ -617,7 +606,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.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_min = 0.05;          // 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);

@@ -625,15 +614,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(

  // 3) Adjust speed using Hermite trajectory curvature + remaining distance
  double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
-  // const double L_min = 0.1;  // m, chỉnh theo nhu cầu
-  // double scale_close = std::clamp(L / L_min, 0.0, 1.0);
-  // v_target *= scale_close;
+  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
+  double scale_close = std::clamp(L / L_min, 0.0, 1.0);
+  v_target *= scale_close;
  const double y_abs = std::fabs(carrot_pose.pose.y);
  const double y_soft = 0.1;
  if (y_abs > y_soft) 
  {
    double scale = y_soft / y_abs;  // y càng lớn => scale càng nhỏ
-    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
+    scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
    v_target *= scale;
    robot_nav_2d_msgs::Twist2D cmd, result;
    cmd.x = v_target;
@@ -644,6 +633,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
  // 4) Maintain minimum approach speed
  if (std::fabs(v_target) < min_approach_linear_velocity)
    v_target = std::copysign(min_approach_linear_velocity, sign_x);
+
  // 5) Angular speed from curvature
  double w_target = v_target * kappa;
  if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -654,20 +644,18 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
      for(int i = trajectory.poses.size() - 2; i >= 0; i--)
      {
        const auto& p = trajectory.poses[i].pose;
-        const auto& dx =  p1.x - p.x ;
-        const auto& dy =  p1.y - p.y ;
-        if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
+        if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
        {
-          if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
-            continue;
-          heading_ref = std::atan2(dy, dx);
+          heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
          if(sign_x < 0.0)
-          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
+          {
+            heading_ref = angles::normalize_angle(M_PI + heading_ref);
+          }
          break;
        }
      }

-      const double error = heading_ref;
+      const double error = angles::normalize_angle(heading_ref);
      double w_heading = 0.0;
      pid(error, 
        near_goal_heading_integral_, 
@@ -680,11 +668,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
      // Apply acceleration limits
      double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
      w_target = velocity.theta + dw_heading;
-      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
    }
    else
    {
-      w_target = std::clamp(w_target, -0.001, 0.001);
+      w_target = 0.0;
      near_goal_heading_was_active_ = false;
    }
  }
@@ -700,27 +687,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(

  drive_cmd.x = velocity.x + dv;
  drive_cmd.theta = velocity.theta + dw;
-  
-  Eigen::VectorXd y(2);
-  y << drive_cmd.x, drive_cmd.theta;
-
-  // 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], -fabs(drive_target.theta), fabs(drive_target.theta));
-  // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
 }

 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -744,9 +710,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
    double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
    target_speed = max_speed * cosine_factor;
  }
-  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
-  double reduce_speed = std::min(max_speed, v_min);
+
+  double reduce_speed = std::min(max_speed, min_speed_xy_);
  if (s < S_final)
  {
    double r = std::clamp(s / S_final, 0.0, 1.0);
@@ -779,29 +744,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
  // const double max_kappa = calculateMaxKappa(global_plan);
  // const bool curvature = max_kappa > straight_threshold;
  double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 4 &&
-  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
+  if(is_stopped && global_plan.poses.size() >= 2)
  {
-    const auto& p1 = global_plan.poses[2];
-    for(int i = 3; i < global_plan.poses.size(); i++)
+    const auto& p1 = global_plan.poses[1];
+    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(dx, dy) > costmap_robot_->getCostmap()->getResolution())
+      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
      {
-        if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
-          continue;
-        path_angle = std::atan2(dy, dx);
+        path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x);
        break;
      }
    }
  }
  
  // Whether we should rotate robot to rough path heading
-  // if(sign_x < 0.0)
-  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
-  // angle_to_path = path_angle;
  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
  double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
  // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
@@ -818,9 +775,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
 #ifdef BUILD_WITH_ROS
  if (result)
    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-#else
-  if (result)
-    robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+
+  // else if(fabs(velocity.x) < min_speed_xy_)
+  // {
+  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
+  //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  // }
 #endif
  return result;
 }
@@ -903,11 +863,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
    for(int i = trajectory.poses.size() - 2; i >= 0; i--)
    {
      const auto& p = trajectory.poses[i].pose;
-      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())
+      if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
      {
-        heading_error = angles::normalize_angle(std::atan2(dy, dx));
+        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);
+        }
        break;
      }
    }
@@ -957,10 +919,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
  // --- Linear velocity calculation ---
  // Base velocity proportional to distance, with minimum for smooth motion
  double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
-  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
  v_base = std::max(v_base, final_heading_min_velocity_);
-  v_base = std::min(v_base, v_min);
+  v_base = std::min(v_base, min_speed_xy_);

  // Scale down when heading error is large (prioritize rotation)
  double heading_scale = 1.0;
@@ -1032,7 +992,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
  cmd_vel.theta = omega_current + domega;

  // --- Apply velocity limits ---
-  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
+  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
  cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);

  // --- Safety: ensure we can stop ---
@@ -1214,11 +1174,9 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
  double v_limit = std::fabs(v_target);
  double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
  
-  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
  if (journey_distance < min_journey_squared_)
  {
-    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
  }

  if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
@@ -1228,7 +1186,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
  }

  if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
-    v_limit = v_min * sign_x;
+    v_limit = min_speed_xy_ * sign_x;

  if (fabs(decel_lim_x_) > 1e-6)
  {
@@ -1246,8 +1204,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
  const robot_nav_2d_msgs::Twist2D &drive_target,
  const robot_nav_2d_msgs::Twist2D &velocity,
  const double &sign_x,
-  robot_nav_2d_msgs::Twist2D &drive_cmd, 
-  const double &dt)
+  robot_nav_2d_msgs::Twist2D &drive_cmd)
 {
  if (path.poses.empty())
  {
@@ -1255,31 +1212,24 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
    drive_cmd.theta = 0.0;
    return robot_nav_2d_msgs::Path2D();
  }
-
-  drive_cmd.x = drive_target.x;
-  drive_cmd.theta = max_vel_theta_;
  double max_kappa = calculateMaxKappa(path);
  const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
-  // nếu đường thẳng
-  if (max_kappa <= straight_threshold) 
+  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
+  drive_cmd.theta = max_vel_theta_;
+
+  if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
  { 
-    if(fabs(path.poses.back().pose.x) < min_lookahead_dist_ * 0.8)
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
    {
-      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
-        drive_cmd.theta = 0.01;
      return generateParallelPath(path, sign_x);
    }
-    return generateHermiteTrajectory(path, sign_x);
+    return generateHermiteTrajectory(path.poses.back(), sign_x);
  }
  else // nếu đường cong
  {
-    const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-    const double v_min = std::min(fabs(v_limited), min_speed_xy_);
-    if(fabs(drive_cmd.x) < v_min) 
-    {
-      drive_cmd.x = std::copysign(v_min, sign_x);
-    }
-    return generateHermiteQuadraticTrajectory(path, sign_x);
+    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);
  }
 }

@@ -1309,47 +1259,40 @@ 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 = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
+    parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
+    parallel_path.poses[i].header = path.poses[i].header;
  }
+
  return parallel_path;
 }

 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
-  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
+  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
 {
  robot_nav_2d_msgs::Path2D hermite_trajectory;
  hermite_trajectory.poses.clear();
-  hermite_trajectory.header = path.header;
+  hermite_trajectory.header.stamp = pose.header.stamp;
+  hermite_trajectory.header.frame_id = pose.header.frame_id;

-  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 x = pose.pose.x;
+  const double y = pose.pose.y;
+  const double theta = pose.pose.theta;
  const double L = std::hypot(x, y);
+
  if (L < 1e-6) {
    robot_nav_2d_msgs::Pose2DStamped pose_stamped;
-    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;
+    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;
    hermite_trajectory.poses.push_back(pose_stamped);
    return hermite_trajectory;
  }
@@ -1383,39 +1326,30 @@ 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_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);
+    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);
  }
  return hermite_trajectory;
 }

 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
-  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
+  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
 {
  robot_nav_2d_msgs::Path2D trajectory;
  trajectory.poses.clear();
-  trajectory.header = path.header;
-  if (path.poses.empty())
-    return trajectory;
+  trajectory.header.stamp = pose.header.stamp;
+  trajectory.header.frame_id = pose.header.frame_id;

-  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 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 double L = std::hypot(x, y);
  if (L < 1e-6)
  {
@@ -1455,8 +1389,6 @@ 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;
--- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
+++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
@@ -70,8 +70,9 @@ 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__, "%.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);
+        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);
        return true;
      }
    }
--- a/src/Algorithms/Packages/global_planners/custom_planner
+++ b/src/Algorithms/Packages/global_planners/custom_planner
--- a/src/Algorithms/Packages/global_planners/dock_planner
+++ b/src/Algorithms/Packages/global_planners/dock_planner
--- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
+++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
@@ -36,6 +36,8 @@ 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;
@@ -119,7 +121,6 @@ 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)", 
@@ -147,21 +148,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)
@@ -174,11 +175,8 @@ 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;
-    const double distance = std::sqrt(dx * dx + dy * dy);
+    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);
@@ -190,13 +188,13 @@ namespace two_points_planner
    }
    else 
    {
-      robot_geometry_msgs::PoseStamped pose = start;
-      pose.pose.position.x += resolution * cos(theta);
-      pose.pose.position.y += resolution * sin(theta);
-      plan.push_back(pose);
-      plan.push_back(goal);
-      return true;
+      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
+      return false;
    }
+
+    // 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);

--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
@@ -52,12 +52,6 @@ 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
     *
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
+++ b/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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+      exit(1);
    }

    this->initializeOthers();
@@ -252,9 +252,8 @@ 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"));
-
-  parent_.setParam(algorithm_nav_name, original_papams_);
  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+  parent_.setParam(algorithm_nav_name, original_papams_);
  nh_algorithm.setParam("allow_rotate", false);
 }

@@ -322,8 +321,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
      nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
      nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);

-      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);

      if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
      {
@@ -394,6 +393,7 @@ 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,17 +401,9 @@ 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;
@@ -450,6 +442,7 @@ 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);
    }
  }

@@ -555,7 +548,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;
@@ -574,50 +567,17 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }

 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &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_()
+    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
+      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
 {
 }

 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  detected_timeout_wt_.stop();
-  delayed_wt_.stop();
-  if (docking_planner_ != nullptr) {
+  if (docking_planner_)
    docking_planner_.reset();
-  }
-  if (docking_nav_ != nullptr) {
+  if (docking_nav_)
    docking_nav_.reset();
-  }
 }

 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
@@ -633,15 +593,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
  {
    try
    {
-      robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
+      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";
      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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
+        exit(1);
      }
      docking_planner_->initialize(name_, costmap_robot_);
      robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -657,15 +616,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
  {
    try
    {
-      robot::PluginLoaderHelper loader;
-      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
+      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";
      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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
+        exit(1);
      }
      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_);
@@ -795,8 +753,7 @@ 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");
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
+++ b/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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+      exit(1);
    }

    robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
+++ b/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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+      exit(1);
    }
    
    std::string algorithm_nav_name; 
@@ -107,21 +107,20 @@ 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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+      exit(1);
    }

    std::string algorithm_rotate_name;
@@ -136,7 +135,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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
+        exit(1);
      }
      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());
@@ -144,7 +143,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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
+      exit(1);
    }

    std::string goal_checker_name;
@@ -160,7 +159,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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+        exit(1);
      }
      goal_checker_->initialize(parent_);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -168,7 +167,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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+      exit(1);
    }

    this->initializeOthers();
@@ -235,15 +234,6 @@ 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_);
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
+++ b/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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
+      exit(1);
    }

    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());
-        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+        exit(1);
      }
      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());
-      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
+      exit(1);
    }
    
    robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
--- a/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
+++ b/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
@@ -89,7 +89,7 @@ public:

 private:
  /**
-   * @brief Resolve library path (absolute, then PNKX_NAV_CORE_LIBRARY_PATH, search_paths_, cwd)
+   * @brief Resolve library path (handle relative paths, search in search_paths)
   * @param library_path Path from config (may be relative or absolute)
   * @return Resolved absolute path, or empty if not found
   */
--- a/src/Libraries/robot_cpp/src/node_handle.cpp
+++ b/src/Libraries/robot_cpp/src/node_handle.cpp
@@ -16,70 +16,6 @@
 #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
 {

@@ -715,8 +651,7 @@ 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;
@@ -1211,14 +1146,92 @@ 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
    {
-      robot_xmlrpcpp::XmlRpcValue v_copy = v;
-      YAML::Node node = xmlRpcToYaml(v_copy);
-      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
+      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;
+      }
    }
    catch (...)
    {
+      // On error, create empty node
      const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
    }
  }
--- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
+++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
@@ -66,7 +66,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
  }
  // Try to read from NodeHandle
  std::string library_path;
-  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
  if (nh_.hasParam(param_path)) {
    nh_.getParam(param_path, library_path, std::string(""));
    if (!library_path.empty()) {
@@ -98,6 +97,7 @@ 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,27 +198,21 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
    return "";
  }

-  // 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) {
+  // 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
    std::string lib_path_with_ext = library_path;
    if (lib_path_with_ext.find(".so") == std::string::npos) {
      lib_path_with_ext += ".so";
    }
-    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();
-        } catch (...) {
-          return full_path.string();
-        }
+    std::filesystem::path full_path = std::filesystem::path(build_dir) / lib_path_with_ext;
+    if (std::filesystem::exists(full_path)) {
+      try {
+        return std::filesystem::canonical(full_path).string();
+      } catch (...) {
+        return full_path.string();
      }
    }
  }
@@ -344,7 +338,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
 std::string PluginLoaderHelper::getWorkspacePath()
 {
  // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
-  const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
  if (workspace_path && std::filesystem::exists(workspace_path)) {
    return std::string(workspace_path);
  }
--- a/src/Libraries/robot_time
+++ b/src/Libraries/robot_time
--- a/src/Libraries/xmlrpcpp
+++ b/src/Libraries/xmlrpcpp
--- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
+++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
@@ -41,7 +41,6 @@
 #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
@@ -141,13 +140,6 @@ namespace robot_nav_core
     */ 
    virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;

-
-    /**
-     * @brief Gets the current global plan
-     * @param path The global plan
-     */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
-
    /**
     * @brief  Constructs the local planner
     * @param name The name to give this instance of the local planner
@@ -156,12 +148,6 @@ 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
     */
@@ -169,11 +155,6 @@ namespace robot_nav_core

  protected:
    BaseLocalPlanner() {}
-
-    /**
-     * @brief The odometry of the robot
-     */
-     robot_nav_msgs::Odometry *odom_;
  };
 } // namespace robot_nav_core

--- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
+++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
@@ -99,12 +99,6 @@ namespace robot_nav_core2
     */ 
    virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;

-    /**
-     * @brief Gets the current global plan
-     * @param path The global plan
-     */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
-
    /**
     * @brief Compute the best command given the current pose, velocity and goal
     *
--- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
+++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
@@ -167,12 +167,6 @@ 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
@@ -195,6 +189,11 @@ 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_;
--- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
+++ b/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,17 +407,6 @@ 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 ||
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -31,9 +31,7 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
-#ifndef BUILD_WITH_ROS
-  #include <laser_filter/laser_filter.h>
-#endif
+#include <laser_filter/laser_filter.h>

 move_base::MoveBase::MoveBase()
  : initialized_(false),
@@ -271,7 +269,6 @@ 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);

@@ -315,7 +312,6 @@ 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);
@@ -326,7 +322,6 @@ 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)
    {
@@ -403,7 +398,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();
@@ -505,7 +500,6 @@ 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
@@ -518,16 +512,6 @@ 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);
@@ -550,13 +534,8 @@ 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)
@@ -1246,47 +1225,6 @@ 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;
@@ -1424,6 +1362,7 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
      lock.unlock();
      return false;
    }
+
    as_->processGoal(action_goal);
  }
  catch (const std::exception &e)
@@ -1783,6 +1722,32 @@ 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)
@@ -2006,7 +1971,6 @@ 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);
@@ -2358,9 +2322,8 @@ 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_);
@@ -2580,6 +2543,7 @@ 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
@@ -2741,7 +2705,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
      recovery_index_ = 0;
  }

-
  // Cancle executeCycle
  if (cancel_ctr_ && tc_)
  {
@@ -2750,25 +2713,13 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
    tc_->setTwistLinear(linear);
    try
    {
-      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_)
+      if (fabs(tc_->getActualTwist().linear.x) <= 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;
+        swapPlanner(default_config_.base_global_planner);
+
        // disable the planner thread
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
@@ -2779,21 +2730,7 @@ 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;
      }
    }
@@ -2802,17 +2739,7 @@ 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);
-      }
+      swapPlanner(default_config_.base_global_planner);

      // disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2824,20 +2751,6 @@ 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;
    }
@@ -2858,6 +2771,7 @@ 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
@@ -2866,7 +2780,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;
@@ -3031,8 +2945,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
        tc_->setTwistLinear(linear);
        try
        {
-          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_)
+          if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
          {
            paused_ = true;
          }
@@ -3072,7 +2985,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_);
@@ -3136,39 +3049,15 @@ 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)
 {
-
-  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);
+  robot_geometry_msgs::PoseStamped goal_pose, global_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());

+  goal_pose.header.stamp = robot::Time(); // latest available
  try
  {
-    // 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);
-    }
+    tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+    tf3::doTransform(goal_pose, global_pose, transform);
  }
  catch (tf3::LookupException &ex)
  {
@@ -3204,21 +3093,6 @@ 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);
Author	SHA1	Message	Date
HiepLM	56ccd202d0	add dock to	2026-03-19 04:25:59 +00:00
HiepLM	e5c04f476b	add log	2026-03-19 04:12:51 +00:00
HiepLM	f02c20cc5c	add some files	2026-03-18 06:51:10 +00:00