Add recovery and mission manager structure

CATKIN_BUILD_README.md

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

CMAKE_BUILD_README.md

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

config/costmap_common_params.yaml

@@ -11,7 +11,7 @@ navigation_map:
   map_file: maze
 
 virtual_walls_map:
-  map_topic: /virtual_walls/map
+  map_topic: /map
   namespace: /virtual_walls
   map_pkg: managerments
   map_file: maze
@@ -19,23 +19,23 @@ 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
-    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
-    data_type: LaserScan
-    clearing: true
-    marking: false
-    inf_is_valid: true
-    min_obstacle_height: 0.0
-    max_obstacle_height: 0.25
+  observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing
+  # f_scan_marking: f_scan_marking f_scan_clearing
+  #   topic: /f_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
+  #   data_type: LaserScan
+  #   clearing: true
+  #   marking: false
+  #   inf_is_valid: true
+  #   min_obstacle_height: 0.0
+  #   max_obstacle_height: 0.25
   b_scan_marking:
     topic: /b_scan
     data_type: LaserScan
@@ -53,4 +53,28 @@ obstacles:
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
 
+  pc_marking:
+    topic: /camera/depth/points_proc
+    data_type: PointCloud2
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.0
+    min_obstacle_height: 0.1
+    max_obstacle_height: 1.0
 
+  pc_clearing:
+    topic: /camera/depth/points_clear
+    data_type: PointCloud2
+    clearing: true
+    marking: false
+    inf_is_valid: false
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.0
+    min_obstacle_height: -0.5
+    max_obstacle_height: 5.0

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.0
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -7,4 +7,4 @@ global_costmap:
         - {name: inflation,  type: "InflationLayer" }
     obstacles:
         enabled: false
-        footprint_clearing_enabled: false
+        footprint_clearing_enabled: true

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.0
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -1,6 +1,7 @@
 local_costmap:
   frame_id: odom
   plugins:
+    # - {name: virtual_walls_map, type: "StaticLayer" }
     - {name: obstacles,  type: "VoxelLayer" }
     - {name: inflation,  type: "InflationLayer" }
   obstacles:

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,64 @@
+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
+    # Robot
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.6
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.6
+    robot_min_v: -0.5
+    robot_min_w: 0.05
+    robot_max_v_backwards_pt: -0.25
+    acc_lim_x: 1.0
+    acc_lim_theta: 2.0
+    min_turn_radius: 0.0
+    turn_around_priority: true
+    stop_dist: 0.5
+    dec_dist: 1.0
+    max_acceleration: 0.1
+    max_deceleration: 0.5
+    max_d_yawrate: 3.2
+    max_in_place_yawrate: 0.6
+    min_in_place_yawrate: 0.3
+    robot_radius: 0.1
+    footprint_padding: 0.08
+    use_footprint: true
+
+    # GoalTolerance
+    xy_goal_tolerance: 0.02
+    yaw_goal_tolerance: 0.02
+
+    # Trajectory
+    max_global_plan_lookahead_dist: 3.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_goal_sep: 0.05
+    global_plan_prune_distance: 0.0
+    predict_time: 3.0
+    sim_period: 0.1
+    sim_time_samples: 10
+    sim_direction: M_PI / 2.0
+
+    # Optimization
+    w_vel: 0.8
+    w_omega: 2.5
+    obs_cost_gain: 1.3
+    path_cost_gain: 2.0
+    to_goal_cost_gain: 0.8
+    speed_cost_gain: 0.8
+
+    # Obstacles
+    obs_range: 2.5
+
+    #GeneralSetting
+    segment_transition_threshold: 0.01
+    calibration_factor: 1.5
+    use_obstacle_avoidance: true
+    slow_velocity_th: 0.1
+    turn_direction_th: 0.1

config/move_base_common_params.yaml

@@ -1,18 +1,43 @@
-position_planner_name: PNKXLocalPlanner
+# position_planner_name: PNKXLocalPlanner
+# docking_planner_name: PNKXDockingLocalPlanner
+# position_planner_name: StanleyLocalPlanner
+# docking_planner_name: StanleyDockingLocalPlanner
+position_planner_name: MPPILocalPlanner #HybridLocalPlanner MPPILocalPlanner
 docking_planner_name: PNKXDockingLocalPlanner
 go_straight_planner_name: PNKXGoStraightLocalPlanner
 rotate_planner_name: PNKXRotateLocalPlanner
 base_local_planner: LocalPlannerAdapter
 base_global_planner: CustomPlanner
 
-PNKXLocalPlanner:
+# PNKXLocalPlanner:
+#   base_local_planner: LocalPlannerAdapter
+#   base_global_planner: SBPLLatticePlanner
+
+# PNKXDockingLocalPlanner:
+#   base_local_planner: LocalPlannerAdapter
+#   base_global_planner: CustomPlanner
+
+# StanleyLocalPlanner:
+#   base_local_planner: LocalPlannerAdapter
+#   base_global_planner: CustomPlanner
+
+# StanleyDockingLocalPlanner:
+#   base_local_planner: LocalPlannerAdapter
+#   base_global_planner: CustomPlanner
+
+# HybridLocalPlanner:
+#   base_local_planner: LocalPlannerAdapter
+#   base_global_planner: CustomPlanner
+
+MPPILocalPlanner:
   base_local_planner: LocalPlannerAdapter
-  base_global_planner: CustomPlanner
+  base_global_planner: SBPLLatticePlanner
 
 PNKXDockingLocalPlanner:
   base_local_planner: LocalPlannerAdapter
   base_global_planner: TwoPointsPlanner
 
+
 PNKXGoStraightLocalPlanner:
   base_local_planner: LocalPlannerAdapter
   base_global_planner: TwoPointsPlanner
@@ -33,7 +58,7 @@ oscillation_distance: 0.5
 recovery_behavior_enabled: true
 recovery_behaviors: [       
                       {name: aggressive_reset, type: ClearCostmapRecovery},
-                      {name: conservative_reset, type: ClearCostmapRecovery},                      
+                      {name: conservative_reset, type: ClearCostmapRecovery}                      
                     ]
 
 conservative_reset:

config/mppi_local_planner_params.yaml

@@ -0,0 +1,126 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017
+  xy_goal_tolerance: 0.03
+  min_approach_linear_velocity: 0.06
+
+MPPILocalPlanner:
+  library_path: libmppi_local_planner
+
+  # Robot limits [m/s, rad/s, m/s^2, rad/s^2]
+  max_velocity: 0.5
+  min_velocity: 0.0
+  max_y_velocity: 0.0
+  min_command_velocity: 0.05
+  max_angular_velocity: 1.9
+  min_angular_velocity: 0.05
+  max_acceleration: 0.5
+  max_angular_acceleration: 1.0
+  max_vel_trans: 0.5
+  vx_std: 0.2
+  vy_std: 0.2
+  wz_std: 0.4
+  motion_model: 0
+  min_turning_radius: 0.2
+
+  # Goal tolerance [m, rad]
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.1
+  stopped_xy_velocity: 0.03
+  stopped_theta_velocity: 0.03
+
+  # MPPI horizon and global-plan handling
+  model_dt: 0.05
+  time_steps: 56
+  batch_size: 1000
+  iteration_count: 1
+  temperature: 0.3
+  gamma: 0.1
+  retry_attempt_limit: 10
+  regenerate_noises: false
+  open_loop: false
+  controller_frequency: 30.0
+  prune_distance: 1.5
+  max_robot_pose_search_dist: 0.0
+  max_global_plan_lookahead_dist: 3.0
+  global_plan_prune_distance: 0.2
+  sim_period: 0.1
+
+  # Costmap safety
+  use_costmap_collision_check: true
+  allow_unknown: true
+  lethal_cost: 253
+  inflation_score_distance: 0.3
+  consider_footprint: true
+
+  # Critic defaults ported from the legacy MPPI dynamic_reconfigure files.
+  constraint_enabled: true
+  constraint_power: 1.0
+  constraint_weight: 4.0
+
+  cost_enabled: true
+  cost_power: 1.0
+  cost_weight: 3.81
+  cost_critical_cost: 300.0
+  cost_near_collision_cost: 253
+  cost_collision_cost: 1000000.0
+  cost_near_goal_distance: 1.0
+  cost_trajectory_point_step: 2
+
+  goal_enabled: true
+  goal_power: 1.0
+  goal_weight: 5.0
+  goal_threshold_to_consider: 1.4
+
+  goal_angle_enabled: true
+  goal_angle_power: 1.0
+  goal_angle_weight: 3.0
+  goal_angle_threshold_to_consider: 0.5
+
+  obstacles_enabled: false
+  obstacles_power: 1.0
+  obstacles_weight: 0.0
+  obstacles_repulsion_weight: 1.5
+  obstacles_collision_cost: 10000.0
+  obstacles_collision_margin_distance: 0.1
+  obstacles_near_goal_distance: 0.5
+
+  path_align_enabled: true
+  path_align_power: 1.0
+  path_align_weight: 8.0
+  path_align_max_path_occupancy_ratio: 0.07
+  path_align_threshold_to_consider: 0.5
+  path_align_offset_from_furthest: 20
+  path_align_trajectory_point_step: 4
+  path_align_use_path_orientations: false
+
+  path_angle_enabled: true
+  path_angle_power: 1.0
+  path_angle_weight: 2.0
+  path_angle_max_angle_to_furthest: 1.2
+  path_angle_threshold_to_consider: 0.5
+  path_angle_offset_from_furthest: 4
+  path_angle_mode: 0
+
+  path_follow_enabled: true
+  path_follow_power: 1.0
+  path_follow_weight: 5.0
+  path_follow_threshold_to_consider: 1.4
+  path_follow_offset_from_furthest: 6
+
+  prefer_forward_enabled: true
+  prefer_forward_power: 1.0
+  prefer_forward_weight: 1.0
+  prefer_forward_threshold_to_consider: 0.5
+
+  twirling_enabled: false
+  twirling_power: 1.0
+  twirling_weight: 0.0
+  twirling_threshold_to_consider: 0.5
+
+  velocity_deadband_enabled: false
+  velocity_deadband_power: 1.0
+  velocity_deadband_weight: 0.0
+  velocity_deadband_deadband_velocities: [0.0, 0.0, 0.0]
+
+  publish_debug: false

config/pnkx_local_planner_params.yaml

@@ -1,5 +1,5 @@
-yaw_goal_tolerance: 0.03  
-xy_goal_tolerance: 0.02 
+yaw_goal_tolerance: 0.02  
+xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
@@ -53,12 +53,12 @@ LimitedAccelGenerator:
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.5
+  acc_lim_x: 3.0
   acc_lim_y: 0.0      # diff drive robot
   acc_lim_theta: 1.5
-  decel_lim_x: -1.5
+  decel_lim_x: -3.0
   decel_lim_y: -0.0
-  decel_lim_theta: -1.5
+  decel_lim_theta: -2.0
 
   # Whether to split the path into segments or not
   split_path: true
@@ -74,8 +74,8 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
-  angle_threshold: 0.47
+  xy_local_goal_tolerance: 0.05
+  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 
@@ -119,7 +119,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
@@ -156,7 +156,7 @@ MKTAlgorithmDiffGoStraight:
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true

config/sbpl_global_params.yaml

@@ -0,0 +1,11 @@
+SBPLLatticePlanner:
+  library_path: libsbpl_lattice_planner
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: true
+  nominalvel_mpersecs: 0.3
+  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
+  primitive_filename: /home/duongtd/T800_ws/src/AMR_T800/Test/sbpl_lattice_planner/matlab/mprim/unicycle_highcost_5cm.mprim

config/stanley_local_planner_params.yaml

@@ -0,0 +1,14 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017  
+  xy_goal_tolerance: 0.03 
+  min_approach_linear_velocity: 0.06
+
+StanleyLocalPlanner:
+  # Algorithm
+  library_path: libstanley_local_planner
+
+StanleyDockingLocalPlanner: 
+  # Algorithm
+  library_path: libstanley_local_planner
+    

src/APIs/c_api/src/nav_c_api.cpp

@@ -569,11 +569,16 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
       return false;
 
     robot_geometry_msgs::Vector3 linear;
-    linear.x = linear_x;
+    linear.x = 0.1;
     linear.y = linear_y;
     linear.z = linear_z;
-    robot::log_info("setTwistLinear %f", linear.x);
-    return nav_ptr->setTwistLinear(linear);
+    bool result = nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Forward %f", linear.x);
+    
+    linear.x = -0.1;
+    result &= result && nav_ptr->setTwistLinear(linear);
+    robot::log_info("setTwistLinear Backward %f", linear.x);
+    return result;
   }
   catch (...)
   {

src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp

@@ -206,7 +206,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // Process index_s with multiple elements
     if (index_s.size() > 1)
     {
-      for (size_t i = 0; i < index_s.size(); ++i)
+      for (size_t i = 1; i < index_s.size(); ++i)
       {
         if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
         {
@@ -219,11 +219,12 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
         double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
 
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
           {
+
             if (index_s[i] > sub_goal_index_saved_)
             {
               sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
@@ -257,7 +258,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
         double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
         double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
         double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
-        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
+        if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.2)
         {
           double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
           if (fabs(tolerance) <= xy_local_goal_tolerance_)
@@ -415,7 +416,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
   robot_nav_2d_msgs::Pose2DStamped sub_pose;
   sub_pose = global_plan.poses[closet_index];
 
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
     geometry_msgs::PoseStamped sub_pose_stamped_ros;
@@ -423,7 +424,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
     sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
     sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
-    sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
+    sub_pose_stamped_ros.pose.position.z = 0.9;
     sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
     sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
     sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
@@ -434,7 +435,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
 
   robot_nav_2d_msgs::Pose2DStamped sub_goal;
   sub_goal = global_plan.poses[goal_index];
-#ifdef SCORE_ALGORITHM_WITH_ROS
+#ifdef BUILD_WITH_ROS
   {
     robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
     geometry_msgs::PoseStamped sub_goal_stamped_ros;
@@ -442,7 +443,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
     sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
     sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
-    sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
+    sub_goal_stamped_ros.pose.position.z = 0.9;
     sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
     sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
     sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;

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

@@ -181,7 +181,7 @@ namespace mkt_algorithm
        */
       robot_nav_2d_msgs::Path2D generateTrajectory(  
         const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
-        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);
 
       /**
        * @brief Generate trajectory

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);
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
     // Normal Pure Pursuit
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     sign_x, 
     dt, 
     drive_cmd);
-  }
+  // }
   applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
 
   if (this->nav_stop_)

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

@@ -368,6 +368,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
+
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -440,13 +441,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
-      else
-        x_direction = 0.0;
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      return false;
+      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      x_direction = x_direction_; 
     }
   }
 
@@ -483,8 +482,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     twist = traj_->nextTwist();
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  // drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
-  drive_cmd.x = sqrt(twist.x * twist.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())
@@ -521,8 +520,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-  allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
-  double angle_to_heading;
+  // 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))
   {
     if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
@@ -551,7 +554,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
 //     else
 //     {
       robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
       
       // Normal Pure Pursuit
@@ -578,6 +581,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
+    
   }
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
@@ -591,7 +595,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
 
   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,15 +625,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   // 3) Adjust speed using Hermite trajectory curvature + remaining distance
   double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
-  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
-  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.2, 1.0); // không giảm quá sâu
+    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
     v_target *= scale;
     robot_nav_2d_msgs::Twist2D cmd, result;
     cmd.x = v_target;
@@ -636,9 +644,6 @@ 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);
-
-  std::stringstream ss;
-
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -656,7 +661,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
           if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
             continue;
           heading_ref = std::atan2(dy, dx);
-          ss << "error " << heading_ref << " ";
           if(sign_x < 0.0)
           heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
@@ -664,7 +668,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       }
 
       const double error = heading_ref;
-      ss << error << " ";
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -676,8 +679,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
         w_heading);
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
-      ss << "dw_heading " << dw_heading << " ";
       w_target = velocity.theta + dw_heading;
+      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
     else
     {
@@ -689,7 +692,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   {
     near_goal_heading_was_active_ = false;
   }
-  
   w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
@@ -698,7 +700,7 @@ 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;
 
@@ -717,8 +719,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
   if (kf_filter_angular_)
-    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
-  // robot::log_info("%s", ss.str().c_str());
+    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(
@@ -742,8 +744,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
     double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
     target_speed = max_speed * cosine_factor;
   }
-
-  double reduce_speed = std::min(max_speed, min_speed_xy_);
+  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);
   if (s < S_final)
   {
     double r = std::clamp(s / S_final, 0.0, 1.0);
@@ -776,10 +779,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   // const double max_kappa = calculateMaxKappa(global_plan);
   // const bool curvature = max_kappa > straight_threshold;
   double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 2)
+  if(is_stopped && global_plan.poses.size() >= 4 &&
+  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
   {
-    const auto& p1 = global_plan.poses[1];
-    for(int i = 2; i < global_plan.poses.size(); i++)
+    const auto& p1 = global_plan.poses[2];
+    for(int i = 3; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
       const auto& dx = p.pose.x - p1.pose.x;
@@ -814,12 +818,9 @@ 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(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);
-  // }
+#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);
 #endif
   return result;
 }
@@ -956,8 +957,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   // --- Linear velocity calculation ---
   // Base velocity proportional to distance, with minimum for smooth motion
   double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   v_base = std::max(v_base, final_heading_min_velocity_);
-  v_base = std::min(v_base, min_speed_xy_);
+  v_base = std::min(v_base, v_min);
 
   // Scale down when heading error is large (prioritize rotation)
   double heading_scale = 1.0;
@@ -1029,7 +1032,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   cmd_vel.theta = omega_current + domega;
 
   // --- Apply velocity limits ---
-  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
+  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
   cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
 
   // --- Safety: ensure we can stop ---
@@ -1211,9 +1214,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   double v_limit = std::fabs(v_target);
   double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
   
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   if (journey_distance < min_journey_squared_)
   {
-    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
   }
 
   if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
@@ -1223,7 +1228,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   }
 
   if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
-    v_limit = min_speed_xy_ * sign_x;
+    v_limit = v_min * sign_x;
 
   if (fabs(decel_lim_x_) > 1e-6)
   {
@@ -1241,7 +1246,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   const robot_nav_2d_msgs::Twist2D &drive_target,
   const robot_nav_2d_msgs::Twist2D &velocity,
   const double &sign_x,
-  robot_nav_2d_msgs::Twist2D &drive_cmd)
+  robot_nav_2d_msgs::Twist2D &drive_cmd, 
+  const double &dt)
 {
   if (path.poses.empty())
   {
@@ -1249,23 +1255,30 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
+
+  drive_cmd.x = drive_target.x;
+  drive_cmd.theta = max_vel_theta_;
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
-  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
-  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
+  // nếu đường thẳng
+  if (max_kappa <= straight_threshold) 
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ )
+    if(fabs(path.poses.back().pose.x) < min_lookahead_dist_ * 0.8)
     {
+      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
+        drive_cmd.theta = 0.01;
       return generateParallelPath(path, sign_x);
     }
     return generateHermiteTrajectory(path, sign_x);
   }
   else // nếu đường cong
   {
-    if(fabs(drive_cmd.x) < min_speed_xy_)
-      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
+    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);
   }
 }
@@ -1310,8 +1323,6 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
   return parallel_path;
 }
 
-
-
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
   const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {

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

@@ -45,6 +45,8 @@ namespace two_points_planner
 
       name_ = name;
       costmap_robot_ = costmap_robot;
+
+      
       
       if(!costmap_robot_ || !costmap_robot_->getCostmap()) 
       {
@@ -55,6 +57,8 @@ namespace two_points_planner
       current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY();
       footprint_ = costmap_robot_->getRobotFootprint();
 
+      robot::log_info("size x: %d, size y: %d, resolution: %f", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY(), costmap_robot_->getCostmap()->getResolution());
+
       robot::log_info("TwoPointsPlanner Initialized successfully");
       initialized_ = true;
       return true;
@@ -119,6 +123,18 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+
+    robot::Time start_time = robot::Time::now();
+    robot::Rate rate(1.0);
+    while(costmap_robot_->getCostmap()->getSizeInCellsX() == 0 || costmap_robot_->getCostmap()->getSizeInCellsY() == 0){
+      robot::log_warning("Waiting for costmap to be initialized...");
+      rate.sleep();
+      if((robot::Time::now() - start_time).toSec() > 5.0){
+        robot::log_error("Costmap not initialized after 10 seconds, exiting...");
+        exit(1);
+      }
+    }
+    robot::log_warning("abc testttttt!, SizeInCellsX = %d, SizeInCellsY = %d", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
     
     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);

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

@@ -366,6 +366,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::c
                                                                                                  const robot_nav_2d_msgs::Twist2D &velocity)
 {
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  robot::log_error("DEBUG 300");
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
   try
   {
@@ -424,9 +425,9 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
     return false;
   }
-
+  robot::log_error("DEBUG 400.1");
   bool dock_ok = dockingHanlde(pose, velocity);
-
+  robot::log_error("DEBUG 400.2");
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
@@ -503,6 +504,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             {
               if (dkpl_.front()->geLocalGoal(local_goal))
               {
+                robot::log_error("DEBUG 100");
                 dkpl_.front()->is_detected_ = true;
                 start_docking_ = true;
                 robot_nav_msgs::Path path;
@@ -524,6 +526,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             {
               if (dkpl_.front()->geLocalGoal(local_goal))
               {
+                robot::log_error("DEBUG 200");
                 dkpl_.front()->is_detected_ = true;
                 start_docking_ = true;
                 robot_nav_2d_msgs::Path2D path;

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

@@ -208,12 +208,14 @@ void pnkx_local_planner::PNKXLocalPlanner::reset()
 void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose)
 {
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  robot::log_error("[PNKXLocalPlanner] Receive new goal(%f, %f)!", goal_pose.pose.x, goal_pose.pose.y);
   reset();
   goal_pose_ = goal_pose;
 }
 
 void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
 {
+  robot::log_error("[PNKXLocalPlanner] size path: %d", (int)path.poses.size());
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
   costmap_robot_->resetLayers();
   global_plan_ = path;

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -174,7 +174,8 @@ namespace robot
 		{
 		public:
 			using Ptr = std::shared_ptr<BaseNavigation>;
-
+			robot_nav_msgs::OccupancyGrid map_save_;
+    		std::string map_name_save_;
 			virtual ~BaseNavigation() {}
 
 			/**

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -125,6 +125,7 @@ namespace move_base
 			* @param laser_scan The laser scan to add.
 			*/
 		 virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) override;
+
  
 		 /**
 			* @brief Add a point cloud to the navigation system.

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

@@ -266,6 +266,26 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
       robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
       throw std::runtime_error("Failed to create the planner_costmap_robot_");
     }
+
+    try
+    {
+      controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+      if(controller_costmap_robot_ == nullptr)
+      {
+        robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__);
+        throw std::runtime_error("Failed to create the controller_costmap_robot_");
+      }
+      controller_costmap_robot_->pause();
+      robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
+      throw std::runtime_error("Failed to create the controller_costmap_robot_");
+    }
+
+    addStaticMap(map_name_save_, map_save_);
+
     // initialize the global planner
     try
     {
@@ -294,22 +314,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
       throw std::runtime_error("Failed to create the " + global_planner + " planner");
     }
 
-    try
-    {
-      controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
-      if(controller_costmap_robot_ == nullptr)
-      {
-        robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__);
-        throw std::runtime_error("Failed to create the controller_costmap_robot_");
-      }
-      controller_costmap_robot_->pause();
-      robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
-    }
-    catch (const std::exception &ex)
-    {
-      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
-      throw std::runtime_error("Failed to create the controller_costmap_robot_");
-    }
     // create a local planner
     try
     {
@@ -487,7 +491,7 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms
   // for(size_t i = 0; i < map.data.size(); i++) {
   //   robot::log_info("map data[%zu]: %d", i, map.data[i]);
   // }
-  // robot::log_info("--------------------------------");
+  robot::log_info("--------------------------------");
   updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
   updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
 }
@@ -729,8 +733,15 @@ void move_base::MoveBase::updateGlobalCostmap(const T& value, robot_costmap_2d::
     {
       // Kiểm tra layer có đúng type và name không
       // Nếu layer type khớp HOẶC name khớp thì update
+      // if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER)
+      //   robot::log_warning ("[%s:%d] Update GlobalCostmap: layer type: %d, name: %s, layer->getType(): %d, layer->getName(): %s", __FILE__, __LINE__, layer_type, name.c_str(), layer->getType(), layer->getName().c_str());
       if (layer->getType() == layer_type || layer->getName() == name)
       {
+        if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER)
+        {
+          robot::log_warning("[%s:%d] Update GlobalCostmap: layer type is STATIC_LAYER, name: %s", __FILE__, __LINE__, name.c_str());
+        }
+
         // Update costmap với data
         layer->dataCallBack<T>(value, name);
       }
@@ -2864,7 +2875,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       if (tc_->isGoalReached())
       {
-        robot::log_debug("Goal reached!");
+        robot::log_debug("[MoveBase] Goal reached!");
         resetState();
         // swapPlanner(default_config_.base_global_planner);
         // disable the planner thread