merge branch 3.0

Merge remote-tracking branch 'origin/3.0' into awc-devel
update find path file plugin .so
2026-03-23 03:03:08 +00:00 · 2026-03-22 10:27:54 +00:00 · 2026-03-22 05:00:23 +00:00 · 2026-03-22 03:00:05 +00:00 · 2026-03-22 02:58:56 +00:00 · 2026-03-21 12:30:57 +00:00
52 changed files with 377 additions and 2849 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/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()
@@ -150,83 +166,6 @@ if (NOT TARGET robot_clear_costmap_recovery)
    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery)
 endif()

-# 3. Packages being migrated from AMR_T800/Test
-#
-# Prefer the future in-tree location.  Until the migration is physically done,
-# keep the current sibling Test directory buildable from this root CMake file.
-option(PNKX_NAV_CORE_BUILD_TEST_PACKAGES
-       "Build navigation packages currently kept in AMR_T800/Test"
-       ON)
-
-option(PNKX_NAV_CORE_BUILD_CATKIN_TEST_PACKAGES
-       "Build Test packages that require a catkin workspace instead of standalone CMake"
-       OFF)
-
-set(PNKX_NAV_CORE_TEST_PACKAGES_DIR ""
-    CACHE PATH "Directory containing the migrated navigation Test packages")
-
-if(NOT PNKX_NAV_CORE_TEST_PACKAGES_DIR)
-    if(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/Test")
-        set(PNKX_NAV_CORE_TEST_PACKAGES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Test")
-    elseif(IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/../Test")
-        set(PNKX_NAV_CORE_TEST_PACKAGES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../Test")
-    endif()
-endif()
-
-function(pnkx_add_test_package package_name)
-    set(package_dir "${PNKX_NAV_CORE_TEST_PACKAGES_DIR}/${package_name}")
-    if(EXISTS "${package_dir}/CMakeLists.txt")
-        message(STATUS "[pnkx_nav_core] Adding migrated Test package: ${package_name}")
-        add_subdirectory("${package_dir}" "${CMAKE_BINARY_DIR}/Test/${package_name}")
-    else()
-        message(WARNING
-                "[pnkx_nav_core] Test package '${package_name}' was not found in "
-                "'${PNKX_NAV_CORE_TEST_PACKAGES_DIR}'.")
-    endif()
-endfunction()
-
-if(PNKX_NAV_CORE_BUILD_TEST_PACKAGES)
-    if(NOT PNKX_NAV_CORE_TEST_PACKAGES_DIR)
-        message(FATAL_ERROR
-                "PNKX_NAV_CORE_BUILD_TEST_PACKAGES is ON, but no Test package directory was found. "
-                "Set PNKX_NAV_CORE_TEST_PACKAGES_DIR explicitly.")
-    endif()
-
-    # Foundation and message packages.
-    pnkx_add_test_package(angles)
-    pnkx_add_test_package(image_geometry)
-    pnkx_add_test_package(grid_map_core)
-    pnkx_add_test_package(vda5050_msgs)
-
-    # Planner libraries and plugins.  SBPL must precede its lattice plugin.
-    pnkx_add_test_package(sbpl)
-    pnkx_add_test_package(sbpl_lattice_planner)
-    pnkx_add_test_package(base_local_planner)
-    pnkx_add_test_package(hybrid_local_planner)
-    pnkx_add_test_package(mppi_local_planner)
-    pnkx_add_test_package(priest_local_planner)
-    pnkx_add_test_package(stanley_local_planner)
-
-    # Supporting runtime packages and test utilities.
-    pnkx_add_test_package(depth_image_proc)
-    pnkx_add_test_package(action_core)
-    pnkx_add_test_package(recovery_core)
-    pnkx_add_test_package(mission_adapters)
-    pnkx_add_test_package(nav_test_harness)
-
-    # Must be last: it consumes planners, actions, recovery and mission APIs.
-    pnkx_add_test_package(move_base2)
-endif()
-
-# These packages call find_package(catkin REQUIRED ...) for T800 packages.
-# Keep them outside the standalone graph until their dependencies export
-# *Config.cmake files, or build them through a catkin workspace.
-if(PNKX_NAV_CORE_BUILD_CATKIN_TEST_PACKAGES)
-    pnkx_add_test_package(deep_mpc_local_planner)
-    pnkx_add_test_package(nav_ros_bridge)
-    pnkx_add_test_package(depth_local_costmap_noetic_test)
-endif()
-
 # 2. Main packages (phụ thuộc vào cores)
 # message(STATUS "[move_base] Shared library configured")
 if (NOT TARGET move_base)
@@ -241,3 +180,5 @@ endif()
 message(STATUS "========================================")
 message(STATUS "All packages configured successfully")
 message(STATUS "========================================")
+
+
--- a/config/config/voxel_layer_params.yaml
+++ b/config/config/voxel_layer_params.yaml
@@ -8,8 +8,4 @@ voxel_layer:
  unknown_threshold: 15.0
  mark_threshold: 0
  combination_method: 1
-  frustum_clearing_enabled: true
-  frustum_clearing_pixel_step: 8
-  frustum_min_range: 0.20
-  frustum_max_range: 3.0
-  frustum_depth_camera_topic: /camera/depth/data
+
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -1,7 +1,5 @@
 robot_base_frame: base_link
 transform_tolerance: 1.0
-performance_metrics_enabled: true
-performance_metrics_period: 5.0
 obstacle_range: 3.0
 #mark_threshold: 1
 publish_voxel_map: true
@@ -13,7 +11,7 @@ navigation_map:
  map_file: maze

 virtual_walls_map:
-  map_topic: /map
+  map_topic: /virtual_walls/map
  namespace: /virtual_walls
  map_pkg: managerments
  map_file: maze
@@ -21,80 +19,55 @@ virtual_walls_map:
  lethal_cost_threshold: 100

 obstacles:
-  observation_sources: b_scan pc_marking pc_clearing pc_r_marking pc_r_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:
+  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
+  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
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_marking:
+    topic: /b_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_clearing:
    topic: /b_scan
    data_type: LaserScan
    clearing: true
-    marking: true
+    marking: false
    inf_is_valid: true
-    frustum_clearing_enabled: false
    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
-    frustum_clearing_enabled: 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/data
-    data_type: DepthCameraData
-    clearing: false
-    marking: true
-    frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 8
-    frustum_min_range: 0.20
-    frustum_max_range: 3.5
-
-  pc_r_marking:
-    topic: /camera_right/depth/points_proc
-    data_type: PointCloud2
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    frustum_clearing_enabled: 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_r_clearing:
-    topic: /camera_right/depth/data
-    data_type: DepthCameraData
-    clearing: false
-    marking: true
-    frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 8
-    frustum_min_range: 0.20
-    frustum_max_range: 3.5
-
-  # Depth camera clearing is handled by VoxelLayer frustum clearing from
-  # /camera/depth/image_raw + /camera/depth/camera_info. /camera_right/depth/data
--- 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: 3.0
+  raytrace_range: 3.5
  resolution: 0.05
  z_resolution: 0.2
  rolling_window: false
--- a/config/costmap_global_params_plugins_no_virtual_walls.yaml
+++ b/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: true
+        footprint_clearing_enabled: 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: 3.0
+  raytrace_range: 3.5
  resolution: 0.05
  z_resolution: 0.15
  z_voxels: 8
--- a/config/costmap_local_params_plugins_no_virtual_walls.yaml
+++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml
@@ -1,7 +1,6 @@
 local_costmap:
  frame_id: odom
  plugins:
-    # - {name: virtual_walls_map, type: "StaticLayer" }
    - {name: obstacles,  type: "VoxelLayer" }
    - {name: inflation,  type: "InflationLayer" }
  obstacles:
--- a/config/hybrid_local_planner_params.yaml
+++ b/config/hybrid_local_planner_params.yaml
@@ -1,55 +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
-    max_forward_velocity: 1.0
-    max_reverse_velocity: 0.25       # [m/s], positive magnitude
-    avoidance_max_velocity: 0.4
-    max_angular_velocity: 0.6
-    min_in_place_angular_velocity: 0.3
-    acc_lim_x: 0.1
-    decel_lim_x: 0.5
+
+    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
-    min_turn_radius: 0.0
-    robot_radius: 0.1
-    footprint_padding: 0.08
-    use_footprint: true
+    turn_around_priority: True
+    stop_dist: 0.5
+    dec_dist: 1.0
+

    # 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
-    predict_time: 3.0
-    sim_period: 0.1
-    sim_time_samples: 10
+    xy_goal_tolerance: 0.1
+    yaw_goal_tolerance: 0.07

    # Optimization
-    w_omega: 2.5
-    obs_cost_gain: 1.3
-    path_cost_gain: 0.5
-    to_goal_cost_gain: 0.8
-    speed_cost_gain: 0.5

-    # Obstacles
-    obs_range: 2.5
+    # 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

-    #GeneralSetting
-    segment_transition_threshold: 0.03    # [m], also used as DWA stop distance for intermediate segments
-    rotation_segment_yaw_threshold: 0.05 # [rad], ignore tiny yaw noise at duplicate positions
-    direction_change_hysteresis: 0.15     # normalized dot product
-    use_obstacle_avoidance: true
-    slow_velocity_th: 0.1
-    turn_direction_th: 0.1
-    avoidance_clear_hold_time: 0.5      # [s]
-    rejoin_blend_time: 0.8              # [s]
-    rejoin_max_velocity: 0.25           # [m/s]
-    rejoin_heading_tolerance: 0.35      # [rad]
-    rejoin_path_tolerance: 0.20         # [m]
-    pp_max_omega_correction: 0.15       # [rad/s]
-    rejoin_acc_lim_theta: 1.0           # [rad/s^2]
-    rejoin_lookahead_distance: 0.8      # [m]
+    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,24 +30,24 @@ 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:
    - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}

  charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: charger
    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
    delay: 1.5 # Cấm sửa không là không chạy được
    timeout: 60
-    vel_x: 0.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/mission_adapters_params.yaml
+++ b/config/mission_adapters_params.yaml
@@ -1,42 +0,0 @@
-# Tham số RUNTIME của mission layer (gói `mission_adapters`).
-#
-# Đây là bản đang có hiệu lực. Bản trong `Test/mission_adapters/test/config/` chỉ phục vụ test và
-# chỉ được đọc khi chạy kèm PNKX_NAV_CORE_CONFIG_DIR trỏ vào đúng thư mục đó.
-
-mission_adapters:
-  # Nguồn mission. Thêm một loại nguồn mới = thêm một entry ở đây + một plugin .so, không phải sửa
-  # code của gói.
-  #   name: tên instance, cũng là namespace param riêng của nó
-  #   type: tên symbol export bằng BOOST_DLL_ALIAS, phải có khoá library_path tương ứng bên dưới
-  mission_sources:
-    - {name: goal_src,    type: GoalSourceAdapter}
-    - {name: vda5050_src, type: VDA5050SourceAdapter}
-
-  # Trần thời gian cho MỘT chặng (navigation + action), tính từ lúc chặng được giao. [s]
-  # 0 = tắt. Quá hạn thì chặng bị đánh dấu thất bại VÀ navigation được bảo dừng — lưới cuối cho
-  # trường hợp navigation không bao giờ báo kết quả về.
-  mission_timeout: 0.0
-
-  # Một chặng thất bại thì xử lý phần còn lại của hàng đợi thế nào.
-  #   true  = xoá sạch hàng đợi (mặc định, an toàn cho tuyến đường tuần tự kiểu VDA5050: không tới
-  #           được node n thì chạy tiếp chặng n+1 là cắt ngang đoạn chưa được cho phép đi)
-  #   false = chỉ bỏ chặng lỗi rồi chạy tiếp. Chỉ đặt false khi các mission trong hàng đợi ĐỘC LẬP
-  #           với nhau, không phải các chặng của cùng một tuyến.
-  clear_queue_on_failure: true
-
-# Param riêng của từng instance nguồn mission, đặt theo `name` ở trên.
-vda5050_src:
-  # Frame gán cho goal/start sinh ra từ nodePosition của VDA5050.
-  # Lưu ý: `mapId` trong VDA5050 là danh tính bản đồ, KHÔNG phải frame TF, nên không dùng làm frame.
-  global_frame: map
-
-# Bảng symbol -> thư viện cho Boost.DLL.
-#
-# Thiếu khoá library_path là nguyên nhân phổ biến nhất của lỗi "plugin build xong nhưng runtime báo
-# không tìm thấy". Tên không có đuôi .so được resolve qua PNKX_NAV_CORE_LIBRARY_PATH / devel/lib /
-# LD_LIBRARY_PATH — tức phải `source devel/setup.bash` trước khi chạy.
-GoalSourceAdapter:
-  library_path: libmission_adapters_goal_source
-
-VDA5050SourceAdapter:
-  library_path: libmission_adapters_vda5050_source
--- a/config/move_base_common_params.yaml
+++ b/config/move_base_common_params.yaml
@@ -1,13 +1,13 @@
-position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
+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

-PriestLocalPlanner:
+PNKXLocalPlanner:
  base_local_planner: LocalPlannerAdapter
-  base_global_planner: CustomPlanner #CustomPlanner SBPLLatticePlanner
+  base_global_planner: CustomPlanner

 PNKXDockingLocalPlanner:
  base_local_planner: LocalPlannerAdapter
@@ -22,25 +22,15 @@ PNKXRotateLocalPlanner:
  base_global_planner: TwoPointsPlanner

 ### replanning
-controller_frequency: 30.0   # run controller at 30.0 Hz
+controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
 planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
 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 của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
-# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
-# `recovery_behaviors` ở đây.
-#
-# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
-# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
-# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
-# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
-# config tranh nhau cùng một tên alias.
-recovery_behavior_enabled: false 
+### recovery behaviors
+recovery_behavior_enabled: true
 recovery_behaviors: [       
                      {name: aggressive_reset, type: ClearCostmapRecovery},
                      {name: conservative_reset, type: ClearCostmapRecovery},                      
--- a/config/mppi_local_planner_params.yaml
+++ b/config/mppi_local_planner_params.yaml
@@ -1,98 +0,0 @@
-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]
-  min_velocity: 0.0
-  # Cao hon reference_velocity mot khoang dem (headroom). Neu de sat ref thi
-  # nua tren cua nhieu sampling bi clamp o max, keo van toc trung binh xuong
-  # (do duoc: ref0.45/max0.50 -> chi dat 0.33; max0.65 -> dat 0.41). Robot chay
-  # duoc >0.6 m/s nen dat 0.65.
-  max_velocity: 0.65
-  max_angular_velocity: 1.0
-  robot_radius: 0.30
-  obstacle_point_radius: 0.08
-  footprint_padding: 0.05
-
-  # MPPI core
-  # Horizon = model_dt * time_steps = 3.6 s ~ 1.8 m @0.5 m/s.
-  # Truoc day 1.4 s (0.7 m) qua ngan: robot chi "thay" vat can khi da sat, khong con
-  # cho de vong qua -> hoac ket hoac lang nhang. Do thuc te: 0.7m/1.2m khong toi dich;
-  # 1.8m toi dich voi quang duong chi dai hon 6% so voi duong thang.
-  # Khong tang dt qua 0.15: dt=0.20 lam dao dong tang gap 4 lan.
-  model_dt: 0.2
-  time_steps: 20
-  batch_size: 220
-  # So thread OpenMP. De mac dinh (= 16 core) thi tranh chap voi gazebo/rviz/perception
-  # lam 1 chu ky solveMPPI cham tu 0.9 ms len ~25 ms.
-  num_threads: 4
-  param_lambda: 100.0
-  param_alpha: 0.98
-  param_exploration: 0.05
-  vx_std: 0.2
-  wz_std: 0.25
-  reference_velocity: 0.48
-
-  # Path and goal
-  # Phai lon hon horizon (1.8 m), neu khong terminal goal_weight se keo robot ve
-  # diem cat cua reference path va lam no giam toc moi chu ky.
-  max_global_plan_lookahead_dist: 3.0
-  xy_goal_tolerance: 0.10
-  yaw_goal_tolerance: 0.10
-
-  # Costmap and cost weights
-  obstacle_range: 2.5
-  obstacle_cost_threshold: 150
-  obstacle_cell_step: 2
-  max_obstacle_points: 180
-  obstacle_influence_radius: 1.0
-  # Cost vat can gio CONG DON tren tung cell (truoc day lay max), nen thang nay phai
-  # nho hon nhieu. Giu 85 voi che do sum lam cost vot len ~180x85, ap dao moi thanh
-  # phan khac -> dao dong manh va di vong xa 45%. Do thuc te tai T=24/dt=0.15:
-  # weight=10 cho dao dong thap nhat va khoang ho 0.48 m (du 13 cm).
-  obstacle_cost_use_sum: true
-  obstacle_weight: 10.0
-  collision_cost: 1000000.0
-  progress_weight: 8.0
-  terminal_progress_weight: 20.0
-  goal_weight: 30.0
-  # Phat v^2: gan 0 vi no truc tiep keo van toc xuong (phan truc giac voi robot
-  # muon chay nhanh). Giu nho de van co chut smoothing.
-  linear_control_weight: 0.2
-  angular_control_weight: 0.2
-  control_change_weight: 1.0
-  # Giam toc khi lech huong: ha tu 0.8 xuong 0.3. Truoc day khi ne vat can
-  # (yaw error lon) robot bo <0.1 m/s. 0.3 van giam toc vao cua nhung khong bo.
-  yaw_error_slowdown: 0.1
-  min_tracking_velocity: 0.08
-
-  # Phat hien ket: MPPI co the tra lenh hop le nhung ~0 mai o cuc tieu dia phuong.
-  # Qua nguong nay controller bao that bai -> move_base PLANNING roi CLEARING.
-  stuck_velocity_threshold: 0.03
-  stuck_omega_threshold: 0.05
-  stuck_patience_cycles: 40
-
-  # --- Covariance thich nghi (giai doan 2) ---
-  # Moi chu ky fit lai do rong nhieu cho TUNG buoc thoi gian tu chinh batch.
-  # vx_std/wz_std o tren chi con la gia tri khoi tao va gia tri sau reset.
-  adaptive_covariance: true
-  gaussian_fitting_lambda: 5.0   # phai < param_lambda de trong so du nhon
-  # San chon bang thuc nghiem: thap hon -> muot hon nhung cham va sat vat can hon.
-  min_vx_std: 0.08
-  max_vx_std: 0.35
-  min_wz_std: 0.10
-  max_wz_std: 0.60
-  # Khi bi ket, no rong sigma dan qua tung chu ky de thoat cuc tieu dia phuong.
-  stuck_sigma_inflation: 1.6
-  hard_obstacle_cost_threshold: 254
-  stage_xy_weight: 60.0
-  stage_yaw_weight: 1.0
-  stage_speed_weight: 12.0
-  terminal_xy_weight: 90.0
-  terminal_yaw_weight: 1.0
-  terminal_speed_weight: 12.0
--- a/config/mprim/mqtt_general.yaml
+++ b/config/mprim/mqtt_general.yaml
@@ -1,8 +1,8 @@
-# MQTT:
-#   Name: T800
-#   Host: 172.20.235.170
-#   Port: 1885
-#   Client_ID: T800
-#   Username: robotics
-#   Password: robotics
-#   Keep_Alive: 60
+MQTT:
+  Name: T800
+  Host: 172.20.235.170
+  Port: 1885
+  Client_ID: T800
+  Username: robotics
+  Password: robotics
+  Keep_Alive: 60
--- 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:
@@ -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: 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,8 +74,8 @@ 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

@@ -106,8 +106,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 +119,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 +143,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 +156,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 +180,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/config/priest_local_planner_params.yaml
+++ b/config/priest_local_planner_params.yaml
@@ -1,56 +0,0 @@
-PriestLocalPlanner:
-  library_path: libpriest_local_planner
-
-  # ================= Horizon / optimizer =================
-  t_fin: 7.0                 # [s] planning horizon
-  num_steps: 30              # trajectory samples over the horizon
-  num_batch: 60              # trajectories per CEM iteration
-  initial_up_sampling: 5     # candidate oversampling factor
-  ellite_num: 40             # elites ranked by projection residual
-  num_warm: 20               # warm-started trajectories between cycles
-  max_proj_iter: 8           # alternating projection iterations
-  max_cem_iter: 4            # CEM iterations per control cycle
-  optimizer_threads: 4       # bounded workers; avoid severe overhead from using all CPU cores
-  solve_time_warn_ms: 30.0   # [ms] leaves margin inside the 30 Hz controller period
-  lateral_std: 0.8           # [m] std of lateral waypoint perturbations
-
-  # ================= Robot limits (differential drive) =================
-  max_velocity: 0.5          # [m/s] forward velocity limit
-  max_angular_velocity: 0.4  # [rad/s] also enforced inside the optimizer as a
-                             # lateral-acceleration clamp |a_n| <= omega_max * v
-  max_acceleration: 0.5      # [m/s^2] used in the trajectory projection
-  reference_velocity: 0.4    # [m/s] desired cruise speed along the path
-  v_eps: 0.05                # [m/s] below this speed the curvature clamp/cost is skipped
-  v_min_seed: 0.05           # [m/s] initial-velocity seed along yaw when the robot is stopped
-
-  # Trajectory -> differential (v, omega) conversion with curvature feedforward:
-  #   omega = clamp(omega_traj, +-max_angular_velocity) - heading_gain * heading_error
-  heading_gain: 1.5          # [1/s] feedback gain on the heading error
-  heading_error_stop: 1.2    # [rad] above this, rotate in place (v = 0)
-  min_in_place_yawrate: 0.2  # [rad/s]
-
-  # ================= Goal / plan =================
-  xy_goal_tolerance: 0.10    # [m]
-  yaw_goal_tolerance: 0.10   # [rad]
-  max_global_plan_lookahead_dist: 3.0  # [m]
-
-  # ================= Obstacles (from local costmap) =================
-  num_obs: 40                # obstacles in the CEM cost
-  num_obs_proj: 30           # closest obstacles inside the projection
-  obstacle_range: 2.5        # [m] costmap scan radius around the robot
-  robot_radius: 0.50         # [m]
-  obstacle_point_radius: 0.08  # [m]
-  footprint_padding: 0.05    # [m]
-  obstacle_cost_threshold: 150
-  obstacle_cell_step: 2
-  max_obstacle_points: 150
-
-  # ================= Cost weights =================
-  weight_smoothness: 0.1
-  weight_track: 0.2
-  weight_obs: 1.2
-  weight_clearance: 3.0      # bounded proximity term in (0,1]: 1 at an obstacle
-                             # centre, 0.5 on the collision boundary, 0 out of range
-  weight_angular: 0.5        # hinge penalty on |omega| > max_angular_velocity
-  cem_lamda: 0.9             # CEM temperature
-  cem_alpha: 0.7             # CEM mean/cov blending factor
--- a/config/recovery_behaviors_params.yaml
+++ b/config/recovery_behaviors_params.yaml
@@ -1,170 +0,0 @@
-# Bộ recovery behavior (gen-2, tick-based) và tham số vận hành của chúng.
-#
-# Đây là cây config RUNTIME. Bản dùng cho test của gói nằm ở
-# `Test/recovery_core/test/config/recovery_behaviors_params.yaml` — sửa tham số vận hành thì sửa ở
-# đây, đừng sửa bản test.
-#
-# Danh sách này được `recovery_core::RecoveryRegistry` đọc; caller (RecoveryRunner của move_base2)
-# truyền namespace `recovery` vào. Khoá `recovery_behaviors:` trong `move_base_common_params.yaml`
-# thuộc về move_base cũ và KHÔNG liên quan tới file này.
-
-recovery:
-  # `behaviors` là registry mọi plugin/instance CÓ THỂ dùng. Thứ tự ở đây chỉ là thứ tự nạp,
-  # không còn quyết định luồng chạy. `routes` bên dưới chọn riêng chuỗi theo trigger.
-  #
-  #   wait   : không di chuyển. Vật cản động (người, xe khác) tự đi qua là xong.
-  #   clear  : xoá vật cản đã tích trong costmap, gần trước rồi xa sau.
-  #   rotate : quay tại chỗ cho costmap nhìn lại xung quanh.
-  #   back_up: LÙI — hướng robot không có sensor, nên xếp cuối cùng.
-  behaviors:
-    - {name: wait,        type: WaitRecovery}
-    - {name: clear,       type: ClearCostmapRecovery}
-    # Đặt schema trước plugin: chưa có `DetourPathRecovery`/library_path thì RecoveryRunner bỏ
-    # entry này cùng warning, và bỏ nó khỏi từng route. Khi plugin SBPL được thêm sau này, không
-    # cần sửa move_base2 hay route.
-    - {name: detour_path, type: DetourPathRecovery}
-    - {name: rotate,      type: RotateRecovery}
-    - {name: back_up,     type: BackUpRecovery}
-
-  # trigger -> tên behavior thử tuần tự; lần lỗi kế tiếp cùng trigger mới tiến tới phần tử tiếp
-  # theo của route đó. Đường quay về sau khi một behavior kết thúc (contract 2026-08-04b):
-  #   - planning_failed        -> quay về PLANNING lập plan lại (không có plan nào để bám);
-  #   - controlling/oscillation-> quay về CONTROLLING bám tiếp GLOBAL PATH CŨ, KHÔNG lập plan lại
-  #                               (thay plan trong lúc chạy là việc riêng của planner_frequency);
-  #                               riêng khi leo thang từ PLANNING vì mất pose thì về PLANNING;
-  #   - behavior họ detour (output kPath, vd DetourPathRecovery) nộp path mới -> path đó được áp
-  #     và robot bám tiếp — cách duy nhất một recovery được thay plan.
-  # Robot đi được quá `oscillation_distance` thì mọi cursor route được reset — ngân sách recovery
-  # đầy lại theo tiến độ thật (2026-08-04).
-  #
-  # `controlling_failed` từ 2026-08-04 nghĩa là MẤT DỮ LIỆU/LỆNH: controller không ra lệnh (kể cả
-  # mất pose) quá controller_patience, hoặc mất pose ngay trong PLANNING. Route chỉ có [wait] là
-  # chủ đích: đứng yên chờ một nhịp cho dữ liệu quay lại; hết wait mà dữ liệu vẫn chưa về thì
-  # cursor cạn -> ABORTED, robot dừng hẳn và báo fail; dữ liệu về thì lập plan chạy tiếp.
-  # `planning_failed` giữ đúng nghĩa "planner CÓ dữ liệu nhưng không tìm được đường".
-  # `path_blocked` / `off_path` là TUỲ CHỌN: xoá hai dòng đó = tắt giám sát tuyến, runtime chạy y
-  # như trước 2026-08-04d. Chúng dùng chung `detour_path` nhưng giữ cursor riêng — mỗi nguyên nhân
-  # một ngân sách, và log phân biệt được vì sao robot dừng.
-  routes:
-    planning_failed:    [wait, clear]
-    controlling_failed: [detour_path]
-    oscillation:        [wait]
-    # Cursor reset (2026-08-05): recovery THÀNH CÔNG + NỘP PATH MỚI (detour) thì cursor của trigger
-    # đó reset NGAY — lần chắn kế tiếp là sự cố mới, detour được chạy lại. Từ chối/thất bại mới ăn
-    # vào ngân sách route. Route được phép LẶP behavior nếu muốn cấp thêm lượt thử giữa các nhịp
-    # wait/nhích-lại-gần.
-    #
-    # Cạn route KHÔNG còn nghĩa là ABORT (contract 2026-08-05e): `path_blocked`/`off_path` là DỰ
-    # BÁO, plan vẫn hợp lệ và controller vẫn ra lệnh được, nên robot bám tiếp plan cũ và trigger bị
-    # KHOÁ tới khi đi được quá `oscillation_distance`. Đường ABORT hợp lệ duy nhất đi qua
-    # `controlling_failed` (controller thật sự hết ra lệnh quá controller_patience) hoặc
-    # `planning_failed`.
-    path_blocked:       [detour_path]
-    off_path:           [detour_path]
-
-  wait:
-    wait_duration: 3.0          # [s] đợi vật cản động đi qua
-
-  # Xoá vật cản trong vùng vuông cạnh reset_distance quanh robot.
-  clear:
-    reset_distance: 3.0         # [m] cạnh vùng xoá
-    invert_area_to_clear: false # false = xoá BÊN TRONG vùng
-    affected_maps: both         # local | global | both
-    layer_names: [obstacles]    # phải khớp `plugins:` của costmap; sai tên -> log kèm tên layer thật
-
-  rotate:
-    full_rotation: true         # quay đủ 2*pi để costmap thấy toàn bộ xung quanh
-    angular_speed: 0.4          # [rad/s] độ lớn; dấu do goal.angle quyết định
-    acc_lim_theta: 0.8          # [rad/s^2] ramp, tránh giật khi có tải
-    sim_granularity: 0.1        # [rad] bước quét footprint dọc cung lúc start
-    timeout: 20.0               # [s] lưới cuối nếu robot bị giữ cơ học
-
-  back_up:
-    backup_distance: 0.1        # [m] quãng lùi mặc định
-    backup_distance_max: 1.0    # [m] trần cứng, chặn cả goal.distance lẫn param
-    linear_speed: 0.1           # [m/s] ĐỘ LỚN; dấu âm (lùi) do plugin đặt
-    acc_lim_x: 0.3              # [m/s^2]
-    timeout: 15.0               # [s]
-
-  # Lập đường vòng quanh vật cản (gói sbpl_recovery, họ kPath) rồi NỐI LẠI plan hiện hành — đường
-  # vòng được bám ngay, không replan (contract 2026-08-04b). Chưa nằm trong route nào ở trên: thêm
-  # `detour_path` vào route cần nó (vd `controlling_failed: [wait, detour_path]`) để kích hoạt.
-  detour_path:
-    # Instance planner RIÊNG, không dùng chung SBPLLatticePlanner của backup: detour gọi makePlan
-    # ĐỒNG BỘ trên control thread, nên `allocated_time` của instance này là trần treo control loop
-    # mỗi tick. Xem khối SBPLDetourPlanner ở cuối file.
-    planner_name: SBPLDetourPlanner   # namespace param riêng (khối bên dưới)
-    planner_symbol: SBPLLatticePlanner  # alias Boost.DLL CÓ THẬT trong .so — libsbpl_lattice_planner
-                                        # chỉ export đúng một symbol này. Khác bộ local planner, nơi
-                                        # mỗi biến thể là một class riêng nên tên instance trùng alias.
-    # "combined" (2026-08-05, vòng 3 — LƯỚI GỘP, xem DETOUR_MERGED_GRID_PLAN.md): snapshot global
-    # làm nền (tường/kệ tĩnh toàn bản đồ) + đè cửa sổ local lên (vật cản sensor tươi), SBPL lập
-    # đường TRÊN BẢN CHỤP GỘP — không giữ mutex costmap nào trong lúc search, map update không bị
-    # bỏ đói dù attempt thua. Một bên báo chắn là chắn; local FREE không xoá tường global.
-    # Cần planner export capability "<planner_symbol>ExternalGrid" (sbpl_lattice_planner có sẵn).
-    # "local" / "global" đơn lẻ vẫn dùng được cho hệ thiếu một trong hai nguồn.
-    costmap_source: combined
-    planning_distance: 1.0            # [m] điểm nối cách robot tối thiểu chừng này dọc plan.
-    # [m] TRẦN quét dọc plan, thuần tuỳ chọn. 0 = quét tới CUỐI PLAN (lưới gộp phủ toàn bản đồ nên
-    # không còn ràng buộc cửa sổ local). Quét luôn tự dừng ở rìa lưới; hai lần sim 2026-08-05 cap
-    # hữu hạn (3.8 rồi 8.0) đều cắt quét ngay sau pose bẩn cuối và báo "không có chỗ sạch" oan.
-    max_rejoin_distance: 0.0
-    sample_step: 0.5                  # [m] khoảng cách dọc plan giữa hai ứng viên điểm nối
-    # [m] Điểm nối phải cách pose bẩn gần nhất CẢ HAI PHÍA dọc plan chừng này, và nằm SAU CỤM CHẮN
-    # ĐẦU TIÊN (không phải pose bẩn cuối toàn tầm — vết bẩn xa trên đuôi plan cũ không được giết
-    # ứng viên hợp lệ giữa hai vật cản; sim 11:00: `last blocked at 7.96 m` -> refuse oan -> ABORT).
-    rejoin_clearance: 0.5
-    attempts_per_run: 3               # số ứng viên thử tối đa; mỗi control cycle thử MỘT ứng viên
-    # Tuyến để NỐI LẠI (2026-08-05e). "reference" = tuyến GỐC của chặng, tức plan do planner sinh và
-    # recovery không thay được: đường vòng luôn quay về tuyến order, và `max_deviation` đo đúng hành
-    # lang fleet đã duyệt. "current" = plan đang bám (hành vi cũ) — sim đo được tuyến bị ăn mòn dần
-    # qua từng lượt detour (rejoin 106/305 -> 130/289 -> 132/273) và robot không bao giờ về tuyến.
-    rejoin_on: reference
-    # [m] Hành lang lệch tuyến cho phép quanh tuyến gốc của order. 0 = KHÔNG giới hạn — giữ 0 cho
-    # tới khi biết dung sai thật của fleet master; đặt số mò còn tệ hơn tắt.
-    max_deviation: 0.0
-    timeout: 10.0                     # [s] trần lượt
-
-# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
-# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
-WaitRecovery:
-  library_path: librecovery_core_wait_recovery
-
-ClearCostmapRecovery:
-  library_path: librecovery_core_clear_costmap_recovery
-
-RotateRecovery:
-  library_path: librecovery_core_rotate_recovery
-
-BackUpRecovery:
-  library_path: librecovery_core_back_up_recovery
-
-DetourPathRecovery:
-  library_path: libsbpl_recovery
-
-# Instance SBPL RIÊNG cho detour — cùng .so với SBPLLatticePlanner, khác tuning.
-#
-# Vì sao không dùng chung: `DetourPathRecovery::onUpdate` gọi makePlan ĐỒNG BỘ trên control thread,
-# nên `allocated_time` ở đây là trần thời gian control loop bị treo mỗi tick — `cancel`/`pause` từ
-# host không được xử lý trong khoảng đó. Bản backup để 10.0 s là hợp lý cho vai trò của nó, nhưng
-# treo loop 10 s thì không.
-#
-# `initial_epsilon` lớn = ARA* trả nghiệm đầu rất nhanh, đường dài hơn tối ưu nhưng đây là đường
-# vòng tạm — nhanh quan trọng hơn ngắn.
-SBPLDetourPlanner:
-  # KHÔNG có `library_path` ở đây: `SBPLDetourPlanner` không phải symbol, nó chỉ là namespace param.
-  # Việc nạp .so đi theo `planner_symbol: SBPLLatticePlanner` ở trên.
-  environment_type: XYThetaLattice
-  planner_type: ARAPlanner
-  allocated_time: 0.4        # [s] TRẦN CỨNG cho một tick recovery
-  initial_epsilon: 2.0
-  force_scratch_limit: 10000
-  forward_search: true
-  # false cho DETOUR, khác bản backup: free heading làm mỗi lượt hỏng chạy HAI lần search (log sim:
-  # "no solution with free start heading, retrying..." = 0.77 s giữ mutex costmap, map update miss
-  # nhịp). Detour chạy khi robot ĐANG BÁM plan nên heading thật đã xuôi theo tuyến — không cần nới.
-  free_start_heading: true
-  nominalvel_mpersecs: 0.3
-  timetoturn45degsinplace_secs: 1.31
-  # Cùng file .mprim với SBPLLatticePlanner: resolution 0.05 m khớp resolution costmap local.
-  primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim
--- a/config/sbpl_global_params.yaml
+++ b/config/sbpl_global_params.yaml
@@ -1,15 +0,0 @@
-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
-  # Bỏ ràng buộc heading xuất phát: local planner đã có bước quay tại chỗ đầu path
-  # (turn_around_priority) nên không cần SBPL vẽ cung quay đầu khi goal ở phía sau.
-  # Nếu không ra nghiệm, planner tự retry một lần với heading thật của robot.
-  free_start_heading: true
-  nominalvel_mpersecs: 0.3
-  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
-  primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim
--- a/config/stanley_local_planner_params.yaml
+++ b/config/stanley_local_planner_params.yaml
@@ -1,115 +0,0 @@
-LocalPlannerAdapter:
-  library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017
-  xy_goal_tolerance: 0.03
-  min_approach_linear_velocity: 0.06
-
-StanleyLocalPlanner:
-  library_path: libstanley_local_planner
-
-  # ================= Robot limits =================
-  max_vel_x: 0.40              # [m/s] forward, > 0
-  min_vel_x: -0.25             # [m/s] reverse, < 0
-  wheel_base: 0.0              # [m] 0 = derive from TF front_axle_frame -> rear_axle_frame
-  front_axle_frame: "steer_link"
-  rear_axle_frame: "base_link"
-
-  # Two SEPARATE steering limits.
-  # Path tracking must not use a large angle: forward_vel_ scales with cos(delta) so it
-  # collapses to speed_base, and vel_steer = v/cos(steer) is guarded to 0 as cos -> 0.
-  # Pivoting in place uses the full 90 deg the hardware has.
-  steer_limit_standstill: 1.5708   # [rad] TO BE MEASURED - ceiling at |v| ~ 0
-  steer_limit_at_max_vel: 0.5000   # [rad] TO BE MEASURED - ceiling at |v| = max_vel_x
-  align_steer_angle: 1.5708        # [rad] steering while pivoting. Literal number, NOT M_PI/2
-  min_steer_angle: 0.20            # [rad] threshold for "steering is engaged"
-  max_steer_rate: 0.90             # [rad/s] TO BE MEASURED - steering servo slew
-
-  # ================= Goal tolerance =================
-  xy_goal_tolerance: 0.030     # [m]  should match LocalPlannerAdapter
-  yaw_goal_tolerance: 0.020    # [rad]
-  goal_sq_dist_tol: 0.05       # [m^2] early exit of the closest-point scan
-
-  # ================= Control law (Stanley) =================
-  path_yaw_mode: "tangent"     # tangent | bearing - "bearing" is the old behaviour, kept for rollback
-  control_gain_e: 2.0          # heading gain (path tangent - robot yaw)
-  control_gain_d: 1.0          # base cross-track gain
-  gain_cte_weight: 0.8         # added to k per |cross-track error| [1/m]
-  gain_heading_weight: 0.5     # added to k per |heading error| [1/rad]
-  gain_min: 0.5                # lower clamp of k
-  gain_max: 1.8                # upper clamp of k
-  soft_min_vel: 0.20           # [m/s] FLOOR of the atan2(k*e, v) denominator. NO ceiling
-  alpha_gain: 0.0              # steering low-pass [0..1]; 0 = off, max_steer_rate is used instead
-  curve_gain_e_bonus: 2.0      # added to control_gain_e in curve mode
-  tangent_window_pts: 3        # points averaged for the tangent when the plan carries no yaw
-
-  # ================= Steering bias (kills the steady-state offset) =================
-  # Enable when the robot tracks parallel to the global path because of a mechanical
-  # zero offset or an IMU bias. Start at 0 and raise by 0.005 per trial.
-  steer_bias_gain: 0.0         # [rad/(m*s)] 0 = OFF. Suggested when enabled: 0.02
-  steer_bias_clamp: 0.05       # [rad] anti-windup ceiling
-  steer_bias_decay: 0.995      # per-cycle bleed when the integrate conditions do not hold
-  steer_bias_min_vel: 0.30     # [m/s] below this, do not integrate
-  steer_bias_max_heading_err: 0.10  # [rad] above this, do not integrate
-
-  # ================= Plan window / lookahead =================
-  max_global_plan_lookahead_dist: 3.0   # [m] window ceiling
-  min_lookahead: 0.50                   # [m] window floor
-  min_lookahead_near_goal: 0.40         # [m] separate floor near the goal
-  dyn_lookahead_gain: 2.50              # [m/(m/s)] lookahead = min + gain*|v|
-  near_goal_freeze_dist: 0.50           # [m] below this: freeze yaw_path, run theta_d only
-
-  # Index continuity thresholds, given in METRES and converted to indices with the
-  # point spacing measured at runtime - the plan resolution is the global planner's
-  # choice, not a constant.
-  # The old code used fixed indices (back=3, fwd=15 ~ 0.75 m on a 0.05 m/point plan) and
-  # therefore never fired: at 1.2 m/s the robot covers 0.04 m = 0.8 index per 33 ms cycle.
-  # Applied to BOTH closest (window anchor) and target_idx_ (inside the core).
-  max_advance_dist: 0.20                # [m] forward travel allowed along the plan per cycle
-  max_back_dist: 0.10                   # [m] backward travel allowed along the plan per cycle
-
-  # ================= Curvature =================
-  high_curvature: 0.30         # [1/m] above: shrink lookahead
-  mid_curvature: 0.20          # [1/m] above: cut the transform window
-  low_curvature: 0.10          # [1/m] above: enter curve mode
-  straight_confirm_steps: 110  # consecutive straight cycles before leaving curve mode
-
-  # ================= Speed profile =================
-  speed_base: 0.30             # [m/s] base of the cosine steering profile
-  curve_max_speed: 0.25        # [m/s] ceiling while curving
-  min_drive_speed: 0.07        # [m/s] floor while driving
-  accel_distance: 2.00         # [m] ramp length after leaving a curve
-  decel_min: 0.05              # [m/s per cycle] deceleration on predicted collision
-  decel_max: 0.10              # [m/s per cycle]
-  collision_horizon_factor: 2.0  # multiplies max_vel_x to get the collision check distance
-
-  # ================= Align / phase =================
-  heading_eps: 0.020           # [rad] heading error accepted as aligned
-  steer_eps: 0.020             # [rad] steering error accepted as "servo arrived"
-  heading_align_tol: 0.050     # [rad] gate for the final straight-line approach (was 0.01 = too tight)
-  near_goal_dist: 1.00         # [m] below this: enable the final straight mode
-  steer_realign_off: 0.503     # [rad] above this: CUT drive (0.16*pi)
-  steer_realign_on: 0.150      # [rad] below this: RESUME drive - must be smaller than _off
-  steering_active: 0.094       # [rad] above this the steering counts as active (0.03*pi)
-  segment_transition_threshold: 0.010  # [m]
-  segment_end_ratio: 0.05      # fraction of the segment left before advancing
-  xy_reached_gate_factor: 3.0  # multiplies xy_goal_tolerance; a 1-point window may only latch inside this
-
-  # ================= TF guard =================
-  # Measured on the robot: 225 map->odom jumps, max 12.381 m, coinciding with the
-  # localisation FinishTrajectory(1) 10:41:18 / AddTrajectory(2) 10:41:21. The source is
-  # outside this package; this is containment, not a cure.
-  tf_lookup_at_pose_stamp: true   # true = look up at pose.header.stamp instead of "latest".
-                                  #        "latest" returns a different entry each call when the
-                                  #        buffer holds two map->odom transforms
-  transform_tolerance: 0.20       # [s] accepted age when looking up by timestamp
-  max_plan_tf_jump_rate: 0.60     # [m/s] threshold as a RATE, multiplied by the real elapsed time
-  max_plan_tf_yaw_jump: 0.20      # [rad] rotation threshold - the old code checked translation only
-  tf_reject_window_cycles: 30     # sliding window; exceeding it means a real relocalisation
-  tf_reject_ratio: 0.5            # reject fraction inside the window that triggers the reset
-
-  # ================= Debug =================
-  debug_level: 1               # 0=off 1=on state change 2=throttled 1 Hz 3=every cycle
-                               # 3 produces >2000 lines/min at 30 Hz - field debugging only
-
-StanleyDockingLocalPlanner:
-  library_path: libstanley_local_planner
--- a/src/APIs/c_api/NAV_C_API_2_GUIDE.md
+++ b/src/APIs/c_api/NAV_C_API_2_GUIDE.md
@@ -1,859 +0,0 @@
-# `nav_c_api_2` — hướng dẫn cho khung C#
-
-Tài liệu cho phía host .NET/C# khi chuyển nền navigation từ `libmove_base.so` sang
-`libmove_base2.so`.
-
-Bám sát mã nguồn tại thời điểm viết:
-`pnkx_nav_core/src/APIs/c_api`, `Test/move_base2`, `pnkx_nav_core/src/Navigations/Packages/move_base`
-(bản cũ, dùng để đối chiếu).
-
---
-
-## 0. Đọc nhanh — 8 việc phải làm ở phía C#
-
-| # | Việc | Vì sao |
-|---|---|---|
-| 1 | Gọi `navigation_add_static_map` **trước** `navigation_initialize` | Planner lattice chặn tới khi costmap có kích thước khác 0, và tự `exit(1)` khi chờ quá lâu — giết cả tiến trình |
-| 2 | Gọi `navigation_set_robot_footprint` **trước** `navigation_initialize` | Bản cũ gọi trước là rơi mất; bản mới cache lại và áp đúng lúc dựng costmap |
-| 3 | Thay `navigation_initialize` bằng `navigation_initialize_checked` | Hàm cũ luôn trả `true`, kể cả khi lõi dựng hỏng |
-| 4 | Thêm `navigation_shutdown` vào đường tắt, **trước** `navigation_destroy` | Bản mới có control thread + thread costmap + thread mission đang chạy |
-| 5 | Bỏ mọi suy đoán từ giá trị trả về của `navigation_set_twist_linear/_angular` | Giờ luôn trả `true` nếu số hợp lệ; tín hiệu "đang cancel" của bản cũ không còn |
-| 6 | Bỏ mọi so sánh chuỗi `feed_back_str`, chuyển sang so enum | Nội dung chuỗi do lõi mới đặt, khác hoàn toàn bản cũ |
-| 7 | Không coi `navigation_pause` trả về là robot đã dừng | Lệnh chỉ được ghi nhận, có hiệu lực ở cycle sau (≤ 33 ms @ 30 Hz) |
-| 8 | Gọi các hàm `navigation_free_*` mới sau mỗi getter cấp bộ nhớ | Trước đây không có hàm giải phóng nào cho grid/scan/cloud/plan — mỗi lời gọi rò một lần |
-
-Ba việc **nên** làm thêm: dùng `navigation_get_state` thay cho `navigation_get_feedback` ở vòng poll
-(không cấp phát chuỗi); dùng `navigation_add_depth_camera_data` để bật clear theo frustum; dùng
-`navigation_move_to_order_v2` nếu fleet master có gửi order update.
-
---
-
-## 1. Bố cục — và các hàm cũ nằm ở đâu
-
-**Một thư viện `.so` duy nhất.** `nav_c_api_2` không phải một thư viện khác, không phải một handle
-khác, không phải một tầng bọc. Nó là thêm file nguồn vào **cùng** thư viện đó.
-
-```
-libnav_c_api.so
-├── nav_c_api.cpp     → 50 hàm cũ   (không sửa, không dịch lại, không bọc)
-├── convertor.cpp     → order_free
-├── nav_c_api_2.cpp   → 13 hàm mới
-└── convertor_2.cpp   → converter cho depth camera
-```
-
-64 symbol, cùng một `.so`, không hàm nào trùng tên hàm nào.
-
-**Vậy các hàm không đổi thì C# gọi thế nào? Y hệt hôm nay, không sửa một dòng.** Cùng
-`DllImport("nav_c_api")`, cùng tên symbol, cùng chữ ký, cùng struct. C# không dùng file header —
-P/Invoke nối theo **tên symbol** trong `.so`, mà tên đó không đổi. Lớp binding hiện có tiếp tục chạy
-nguyên trạng.
-
-Cái C# phải thêm chỉ là **13 `DllImport` mới + 4 struct mới** (phụ lục B). Cái C# phải *sửa* là hành
-vi, không phải khai báo — đúng 8 việc ở mục 0.
-
-Hai header chỉ dành cho bên gọi C/C++:
-
-| File | Nội dung |
-|---|---|
-| `include/nav_c_api.h` | Bề mặt cũ. Không đổi chữ ký hàm nào. |
-| `include/nav_c_api_2.h` | Phần thêm. `#include` luôn `nav_c_api.h`, nên bên gọi C/C++ chỉ cần include file này. |
-
-Tên thư viện sinh ra là `libnav_c_api.so` (target `nav_c_api`), nên phía C# là
-`[DllImport("nav_c_api")]`. Tài liệu cũ có chỗ ghi `libnavigation_c_api.so` — kiểm tra lại chuỗi
-khung hiện tại đang dùng.
-
-Mọi hàm dùng `CallingConvention.Cdecl`. `bool` của C++ là 1 byte, phải khai
-`[return: MarshalAs(UnmanagedType.I1)]`. `size_t` trên Linux x64 là 8 byte — dùng `UIntPtr`.
-
---
-
-## 2. Điều kiện vận hành trước khi chạy
-
-| Việc | Giá trị |
-|---|---|
-| `PNKX_NAV_CORE_CONFIG_DIR` | Trỏ vào `Test/move_base2/config/runtime` — **không** phải `pnkx_nav_core/config` |
-| YAML chọn plugin | `MoveBase: library_path: libmove_base2` (đã có sẵn trong `move_base_common_params.yaml`) |
-| Đường tìm `.so` | `libmove_base2.so` và toàn bộ plugin planner/recovery/costmap phải nằm trong `PNKX_NAV_CORE_LIBRARY_PATH`, `devel/lib` hoặc `LD_LIBRARY_PATH` |
-| Nhịp control loop | `controller_frequency: 30.0` — mọi độ trễ "một cycle" nói trong tài liệu này là ~33 ms |
-
-`navigation_create()` tra symbol `"MoveBase"`. `move_base2` export cả `MoveBase` lẫn `MoveBase2`, nên
-đổi runtime chỉ tốn một dòng YAML, không phải sửa C#.
-
---
-
-## 3. Trình tự khởi tạo
-
-Thứ tự này **bắt buộc**, không phải khuyến nghị. Sai thứ tự ở bước 3 làm chết cả tiến trình.
-
-```
-1. tf3_buffer_create()                    → có buffer TF
-2. navigation_create()                    → nạp libmove_base2.so, chưa chạy gì
-3. bơm TF liên tục vào buffer             → bắt đầu NGAY, chạy suốt vòng đời
-4. navigation_set_robot_footprint()       → trước initialize
-5. navigation_add_static_map("/map", ...) → BẮT BUỘC trước initialize
-6. navigation_initialize_checked()        → nặng, chặn, tự start control thread
-7. kiểm tra kết quả trả về                → false thì đọc navigation_get_status_text()
-8. bắt đầu vòng bơm sensor định kỳ
-9. từ đây mới gửi goal
-```
-
-```csharp
-// ---- 1..2 -------------------------------------------------------------------------------
-tfHandle = tf3_buffer_create();
-navHandle = navigation_create();
-if (navHandle == IntPtr.Zero)
-    throw new InvalidOperationException("navigation_create failed — kiểm tra library_path của MoveBase");
-
-// ---- 3 ----------------------------------------------------------------------------------
-// TF phải chảy TRƯỚC initialize: costmap lấy pose robot ngay trong chu kỳ cập nhật đầu tiên.
-StartTfPump();
-
-// ---- 4 ----------------------------------------------------------------------------------
-// Footprint theo chiều ngược kim đồng hồ, toạ độ trong frame base_link, đơn vị mét.
-var footprint = new[]
-{
-    new Point { x =  0.3, y = -0.2, z = 0.0 },
-    new Point { x =  0.3, y =  0.2, z = 0.0 },
-    new Point { x = -0.3, y =  0.2, z = 0.0 },
-    new Point { x = -0.3, y = -0.2, z = 0.0 },
-};
-navigation_set_robot_footprint(navHandle, footprint, (UIntPtr)footprint.Length);
-
-// ---- 5 ----------------------------------------------------------------------------------
-// Tên "/map" phải khớp map_topic trong costmap_common_params.yaml.
-navigation_add_static_map(navHandle, "/map", occupancyGrid);
-
-// ---- 6..7 -------------------------------------------------------------------------------
-// Chặn hàng giây: dựng 2 costmap, nạp planner/controller/recovery, dựng mission layer,
-// start control thread. Không gọi từ UI thread.
-if (!navigation_initialize_checked(navHandle, tfHandle))
-{
-    IntPtr reason = navigation_get_status_text(navHandle);
-    string text = Marshal.PtrToStringAnsi(reason) ?? "unknown";
-    nav_c_api_free_string(reason);
-    throw new InvalidOperationException($"navigation core not ready: {text}");
-}
-
-// ---- 8..9 -------------------------------------------------------------------------------
-StartSensorPumps();
-```
-
-**Vì sao static map phải đi trước.** `initialize()` nạp planner ở pha hai.
-`SBPLLatticePlanner::initialize` chặn tới khi costmap báo kích thước khác 0, mà kích thước đó chỉ có
-khi một static map đã tới `StaticLayer`. Nạp planner trước khi map vào được là khoá chết: planner chờ
-map, map chờ planner xong. SBPL `exit(1)` sau 2 giây. `move_base2` xử lý bằng cách cất static map đã
-nhận rồi **phát lại** ngay trước khi nạp planner — nhưng nó chỉ phát lại được cái host đã gửi.
-
-**Về `map_save_` / `map_name_save_`.** Hai biến public đó là cách bản cũ bù cho đúng vấn đề trên. Với
-`move_base2` host **không cần** đụng tới chúng.
-
---
-
-## 4. Bơm dữ liệu: cái gì, tên gì, tần số nào
-
-Tên truyền vào các hàm `navigation_add_*` phải khớp **giá trị `topic`** khai trong
-`Test/move_base2/config/runtime/costmap_common_params.yaml`, không phải khoá của observation source.
-Sai tên thì dữ liệu được nhận rồi bị bỏ ở tầng layer, im lặng.
-
-| Dữ liệu | Hàm | Tên (`topic` trong YAML) | Tần số | Bắt buộc |
-|---|---|---|---|---|
-| Bản đồ tĩnh | `navigation_add_static_map` | `/map` | Khi bản đồ đổi | **Có** — và trước `initialize` |
-| Laser sau | `navigation_add_laser_scan` | `/b_scan` | Theo nguồn (~10–40 Hz) | Có, nếu dùng laser |
-| Point cloud depth | `navigation_add_point_cloud2` | `/camera/depth/points_proc` | Theo camera | Tuỳ cấu hình |
-| Point cloud depth phải | `navigation_add_point_cloud2` | `/camera_right/depth/points_proc` | Theo camera | Tuỳ cấu hình |
-| Depth camera (clear frustum) | `navigation_add_depth_camera_data` | `/camera/depth/data` | Theo camera (~10–15 Hz) | Cần nếu muốn clear ghost |
-| Depth camera phải | `navigation_add_depth_camera_data` | `/camera_right/depth/data` | Theo camera | Cần nếu muốn clear ghost |
-| Odometry | `navigation_add_odometry` | tên tự do | Theo nguồn odometry | **Có** |
-| TF | qua `tf3` buffer, không qua API này | — | Liên tục | **Có** |
-
-Ghi chú quan trọng:
-
- **Odometry giờ là dữ liệu điều khiển, không phải dữ liệu hiển thị.** `move_base2` lấy vận tốc đo
-  được từ đó và đẩy xuống controller mỗi cycle. Bơm thưa hoặc bỏ qua là controller mất feedback vận
-  tốc.
- **Nguồn `/camera/depth/data` khai `data_type: DepthCameraData` và `frustum_clearing_enabled: true`.**
-  Chỉ `navigation_add_depth_camera_data` đẩy vào được nguồn này. Point cloud không thay thế được —
-  không có intrinsics thì không dựng được frustum, không clear được.
- **Không bơm nhanh hơn nguồn thật.** Đường depth tốn CPU nhất trong các đường sensor.
- **Toàn bộ đường sensor không thread-safe.** Xem mục 8.
-
---
-
-## 5. Trình tự tắt
-
-```csharp
-public void Dispose()
-{
-    // 1. Dừng MỌI thread C# đang gọi navigation_add_* và đợi chúng thoát hẳn.
-    sensorCts.Cancel();
-    Task.WaitAll(sensorTasks, TimeSpan.FromSeconds(2));
-
-    // 2. Quiesce lõi khi tiến trình còn sống đầy đủ. Trả về trong khoảng 1 giây.
-    //    Sau bước này lõi không phát lệnh vận tốc nữa, nhưng getter vẫn gọi được.
-    navigation_shutdown(navHandle);
-
-    // 3. Thả instance.
-    navigation_destroy(navHandle);
-    navHandle = IntPtr.Zero;
-
-    // 4. Buffer TF sau cùng — lõi còn tham chiếu nó cho tới hết bước 3.
-    tf3_buffer_destroy(tfHandle);
-    tfHandle = IntPtr.Zero;
-}
-```
-
-Không được đảo bước 1 và 2, và không được để `navigation_destroy` chạy trong finalizer/GC: thời điểm
-đó do runtime .NET quyết định, có thể rơi vào lúc nó đã tháo dỡ một phần trong khi control thread vẫn
-đang chạy.
-
---
-
-## 6. Tham chiếu từng hàm
-
-Ký hiệu cột "Đổi so với bản cũ": **=** giữ nguyên · **~** cùng chữ ký, khác hành vi · **+** hàm mới.
-
-### 6.1. Vòng đời
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `NavigationHandle navigation_create(void)` | = |
-
-Nạp `libmove_base2.so` qua symbol `MoveBase` và tạo một instance. Trả `NULL` khi không tìm được
-library (sai `library_path`, sai `PNKX_NAV_CORE_CONFIG_DIR`, hoặc `.so` không nằm trong đường tìm).
-Chưa chạy thread nào.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_initialize(NavigationHandle, TFListenerHandle)` | ~ |
-
-Dựng costmap, nạp planner/controller/recovery, dựng mission layer, start control thread. **Chặn**,
-tốn hàng giây. **Luôn trả `true`** — contract bên dưới trả `void` nên không có gì để trả về. Dùng
-`navigation_initialize_checked` thay thế.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_initialize_checked(NavigationHandle, TFListenerHandle)` | + |
-
-Như trên, nhưng đọc lại cờ sẵn sàng của lõi và trả về đúng kết quả thật. Trả `false` khi bất kỳ pha
-nào hỏng: dựng costmap, cấu hình sensor, nạp plugin, cấu hình control loop, start control thread.
-Lý do cụ thể lấy bằng `navigation_get_status_text`.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_is_ready(NavigationHandle)` | + |
-
-Lõi đã sẵn sàng nhận goal chưa. Rẻ, poll được. Không cấp phát gì.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `void navigation_shutdown(NavigationHandle)` | + |
-
-Dừng control thread, thread cập nhật costmap và thread mission — **không huỷ instance**. Sau lời gọi
-này mọi getter vẫn an toàn. Idempotent. Không ném exception ra ngoài biên C.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `void navigation_destroy(NavigationHandle)` | ~ |
-
-Thả instance. Bản cũ gọi thẳng là chấp nhận được vì nó không override `shutdown()`. Bản mới **phải**
-gọi `navigation_shutdown` trước.
-
-### 6.2. Trạng thái và phản hồi
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_feedback(NavigationHandle, NavFeedback &out)` | ~ |
-
-Trả trạng thái, chuỗi mô tả, pose 2D hiện tại, cờ `goal_checked`, cờ `is_ready`.
-
- `feed_back_str` được `strdup`, **phải** giải phóng bằng `nav_c_api_free_string`.
- **Nội dung `feed_back_str` khác bản cũ** — là chuỗi lý do của control loop mới. Chỉ dùng để log.
- `navigation_state` là nguồn duy nhất nên rẽ nhánh.
- Giữa hai chặng của một order nhiều node, lõi ở `SUCCEEDED`/`PENDING` nhưng API **cố ý báo `ACTIVE`**.
-  Nhờ vậy suy "order xong" từ `SUCCEEDED` vẫn đúng — đừng tìm cách đi vòng qua lớp che này.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_state(NavigationHandle, NavigationState *out)` | + |
-
-Chỉ lấy enum trạng thái, không cấp phát chuỗi. Dùng cho vòng poll. `navigation_get_feedback` gọi ở
-10 Hz nghĩa là 10 lần `strdup` + 10 lần `free` mỗi giây, và một lần quên `free` là một lần rò.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `char *navigation_get_status_text(NavigationHandle)` | + |
-
-Chuỗi mô tả của lõi, hoặc `NULL`. Giải phóng bằng `nav_c_api_free_string`. Chỉ để log — nội dung
-không phải hợp đồng.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `char *navigation_state_to_string(NavigationState)` | = |
-
-Tên trạng thái dạng chuỗi. Giải phóng bằng `nav_c_api_free_string`. (Hàm này có export nhưng chưa
-được khai báo trong `nav_c_api.h` — khai báo thủ công phía C# nếu cần.)
-
-### 6.3. Footprint
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_set_robot_footprint(NavigationHandle, const Point *pts, size_t n)` | ~ |
-
-Đặt hình robot, toạ độ trong frame `base_link`, đơn vị mét, thứ tự ngược kim đồng hồ.
-
- **Bản cũ:** áp ngay vào hai costmap; gọi trước `initialize` thì **rơi mất im lặng**.
- **Bản mới:** ghi nhận rồi áp trong `initialize` và ở cycle sau nếu đổi. **Nên gọi trước
-  `initialize`** để planner đầu tiên thấy đúng hình robot.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_robot_footprint(NavigationHandle, Point *out, size_t &out_count)` | ~ |
-
- **Bản cũ:** trả footprint của costmap — có giá trị (từ YAML) kể cả khi host chưa từng set.
- **Bản mới:** trả đúng cái host đã set. **Rỗng nếu host chưa gọi `set_robot_footprint`.**
-
-Cảnh báo về chữ ký: mảng do bên gọi cấp mà **không có tham số dung lượng** — hàm không biết mảng có
-đủ chỗ không. Cấp dư (ví dụ 64 điểm) khi gọi.
-
-### 6.4. Lệnh di chuyển
-
-Tất cả trả `true` khi lệnh được nhận, không phải khi robot tới nơi. Theo dõi kết quả bằng
-`navigation_get_state`.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_move_to(NavigationHandle, PoseStamped goal)` | ~ |
-
-Đi tới một pose. Quaternion không hợp lệ bị từ chối ngay tại API. Trong `move_base2` goal này đi qua
-mission layer như một mission một chặng, để dùng chung đường huỷ và vòng đời với order VDA5050; mission
-layer tắt thì rơi xuống đường trực tiếp.
-
-Dung sai `xy`/`yaw` đến từ profile trong YAML, không đặt được theo từng goal (bản cũ có tham số dung
-sai trong contract nhưng `c_api` chưa bao giờ truyền).
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_move_to_order(NavigationHandle, Order order, PoseStamped goal)` | ~ |
-
-Gửi order VDA5050 đầy đủ. Trong `move_base2` order được **cắt thành từng chặng** tại các node có
-action, lọc theo cờ `released`, và nối tiếp được khi fleet master release thêm horizon. `orderId` và
-`orderUpdateId` là định danh order ở tầng mission — điền đúng.
-
-Nếu `Order` lấy từ `convert2COrder()` thì gọi `order_free()` khi xong.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_move_to_nodes_edges(NavigationHandle, const Node*, size_t, const Edge*, size_t, PoseStamped)` | ~ |
-
-Tiện ích: gửi order chỉ từ mảng node/edge. **Không đặt được `orderId`/`orderUpdateId`** — cả hai để
-rỗng/0, nên mission layer không phân biệt được order update với order mới. Dùng
-`navigation_move_to_order_v2` nếu cần order update.
-
-> Bản trước của hàm này dựng `Order` **không khởi tạo**, khiến bộ chuyển đổi đọc con trỏ rác trên
-> stack. Đã sửa. Nếu khung C# đang chạy với `.so` cũ và thấy crash ngẫu nhiên hoặc chuỗi vô nghĩa ở
-> đường order, đây là nguyên nhân.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_move_to_order_v2(NavigationHandle, const char *order_id, uint32_t order_update_id, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |
-
-Như trên nhưng có định danh thật. Quy ước: cùng `order_id` + `order_update_id` tăng dần = cập nhật
-tuyến đang chạy; đổi `order_id` = order mới thay thế hoàn toàn. `order_id` rỗng bị từ chối.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_dock_to(NavigationHandle, const char *marker, PoseStamped goal)` | = |
-| | `bool navigation_dock_to_order(NavigationHandle, Order, const char *marker, PoseStamped)` | = |
-| | `bool navigation_dock_to_nodes_edges(...)` | ~ (cùng vấn đề `Order`, đã sửa) |
-| | `bool navigation_dock_to_order_v2(NavigationHandle, const char *marker, const char *order_id, uint32_t, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |
-
-Docking dùng profile riêng (`docking_planner_name`) và tên marker. Docking **không** đi qua mission
-layer — nó xuống thẳng lõi.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_move_straight_to(NavigationHandle, double distance)` | = |
-
-Đi thẳng theo hướng hiện tại. Tham số là **khoảng cách [m]**, không phải pose: hàm tự đọc pose robot
-rồi tính goal. Âm là lùi. Vì nó đọc pose ngay lúc gọi nên TF phải sẵn sàng.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_rotate_to(NavigationHandle, PoseStamped goal)` | = |
-
-Quay tại chỗ tới hướng của `goal` (chỉ dùng phần orientation).
-
-### 6.5. Vòng đời nhiệm vụ
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `void navigation_pause(NavigationHandle)` | ~ |
-| | `void navigation_resume(NavigationHandle)` | ~ |
-| | `void navigation_cancel(NavigationHandle)` | ~ |
-
- **Bản cũ:** đồng bộ, có hiệu lực ngay trong lời gọi; ném `std::runtime_error` nếu lõi chưa dựng
-  xong (C API nuốt, C# thấy im lặng).
- **Bản mới:** chỉ ghi nhận yêu cầu; control thread áp ở cycle kế tiếp (≤ 33 ms @ 30 Hz). Không bao
-  giờ ném.
-
-Hệ quả: **trả về ≠ robot đã dừng.** Muốn biết đã dừng, chờ `navigation_get_state` báo `PAUSED`, hoặc
-theo dõi `navigation_get_twist`. Nếu C# có tầng an toàn giả định dừng tức thì thì phải sửa ở phía C#.
-
-`cancel` từ host huỷ **cả order** (gồm hàng đợi mission), khác với huỷ nội bộ do chính mission layer
-phát ra chỉ dừng chặng đang chạy.
-
-### 6.6. Trần vận tốc
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_set_twist_linear(NavigationHandle, double x, double y, double z)` | ~ |
-| | `bool navigation_set_twist_angular(NavigationHandle, double x, double y, double z)` | ~ |
-
-**Đây là trần vận tốc, không phải lệnh jog.** Đường này để tầng an toàn hạ tốc độ robot.
-
- **Dấu của `x` tuyến tính chọn chiều**: `x < 0` đặt trần lùi, còn lại đặt trần tiến. Hai trần được
-  giữ riêng.
- Đơn vị: tuyến tính `[m/s]`, góc `[rad/s]`.
- `NaN`/`Inf` bị từ chối, trả `false`.
- Có hiệu lực ở cycle kế tiếp.
- **Bản cũ** đẩy thẳng xuống controller và trả `false` khi đang có cancel (kèm `unlock()` nội bộ).
-  **Bản mới** chỉ kiểm tra số: giá trị hợp lệ luôn trả `true`. Tín hiệu "đang cancel" **không còn tồn
-  tại** — C# nào dựa vào nó phải chuyển sang đọc trạng thái.
-
-### 6.7. Truy vấn pose và vận tốc
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_robot_pose_stamped(NavigationHandle, PoseStamped &out)` | = |
-| | `bool navigation_get_robot_pose_2d(NavigationHandle, Pose2D &out)` | = |
-
-Pose robot trong global frame. Trả `false` khi TF chưa đủ. Bản `PoseStamped` cấp `frame_id` bằng
-`strdup` — giải phóng bằng `nav_c_api_free_string`.
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_twist(NavigationHandle, Twist2DStamped &out)` | ~ |
-
-**Lệnh vận tốc đang phát**, không phải vận tốc đo được. Đây là thứ host publish ra `cmd_vel`.
-
-Bản mới có cửa ân hạn: khi không có yêu cầu nào chạy, **stamp đứng yên**; stamp bằng 0 nghĩa là lõi
-chưa từng điều khiển. C# nên kiểm tra stamp trước khi publish, tránh đè lên teleop.
-
-`out.header.frame_id` được `strdup` — giải phóng bằng `nav_c_api_free_string`.
-
-### 6.8. Bơm dữ liệu sensor vào
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_add_static_map(NavigationHandle, const char *name, OccupancyGrid)` | ~ |
-| | `bool navigation_add_laser_scan(NavigationHandle, const char *name, LaserScan)` | = |
-| | `bool navigation_add_point_cloud(NavigationHandle, const char *name, PointCloud)` | = |
-| | `bool navigation_add_point_cloud2(NavigationHandle, const char *name, PointCloud2)` | = |
-| | `bool navigation_add_odometry(NavigationHandle, const char *name, Odometry)` | ~ |
-| | `bool navigation_add_depth_camera_data(NavigationHandle, const char *topic, DepthCameraData)` | + |
-
-Chung cho cả nhóm: dữ liệu được **copy** ngay trong lời gọi, buffer C# giải phóng được ngay sau khi
-hàm trả về. `name` phải khớp `topic` trong YAML costmap.
-
- `navigation_add_static_map`: gọi được **trước** `initialize`, và bắt buộc phải thế (mục 3).
- `navigation_add_odometry`: giờ là nguồn vận tốc đo được của controller, không còn chỉ để hiển thị.
- `navigation_add_depth_camera_data` (mới): đường duy nhất tới `VoxelLayer` để **clear theo frustum**
-  — xoá vật cản ma nằm trong tầm nhìn camera. Yêu cầu:
-  - `header.frame_id` là **optical frame** của camera, và TF từ frame đó tới base frame phải có trong
-    buffer tf3 tại đúng `header.stamp`;
-  - `depth.encoding` đúng thật (`"16UC1"` hoặc `"32FC1"`), `step` là số byte mỗi hàng;
-  - `camera_info` là intrinsics của **đúng độ phân giải đang gửi**, không phải của stream màu;
-  - ảnh depth và camera info **cùng một thời điểm** — ghép lệch hai luồng là clear sai vùng.
-
-### 6.9. Đọc lại và xoá dữ liệu sensor
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_static_map(NavigationHandle, const char *name, OccupancyGrid &out)` | = |
-| | `bool navigation_get_laser_scan(NavigationHandle, const char *name, LaserScan &out)` | = |
-| | `bool navigation_get_point_cloud(NavigationHandle, const char *name, PointCloud &out)` | = |
-| | `bool navigation_get_point_cloud2(NavigationHandle, const char *name, PointCloud2 &out)` | = |
-| | `bool navigation_get_all_static_maps(NavigationHandle, NamedOccupancyGrid *out, size_t &n)` | = |
-| | `bool navigation_get_all_laser_scans(...)`, `..._point_clouds(...)`, `..._point_cloud2s(...)` | = |
-| | `bool navigation_remove_static_map / _laser_scan / _point_cloud / _point_cloud2` | = |
-| | `bool navigation_remove_all_static_maps / _laser_scans / _point_clouds / _point_cloud2s` | = |
-| | `bool navigation_remove_all_data(NavigationHandle)` | = |
-
-Các getter **cấp bộ nhớ** — dùng `navigation_free_*` (mục 6.11) sau khi xong.
-
-Bản mới cất **bản đã lọc** của laser scan, đúng bản mà costmap nhìn thấy, nên getter và costmap không
-còn lệch nguồn.
-
-Cảnh báo về chữ ký `get_all_*`: mảng do bên gọi cấp, **không có tham số dung lượng**. Cấp dư.
-
-### 6.10. Dữ liệu hiển thị
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_get_global_data(NavigationHandle, PlannerDataOutput *out)` | ~ |
-| | `bool navigation_get_local_data(NavigationHandle, PlannerDataOutput *out)` | ~ |
-
-Trả plan, lưới costmap (hoặc bản update), cờ `is_costmap_updated`, và footprint. Hàm **tự giải phóng
-nội dung cũ** của struct được truyền vào, nên tái sử dụng một struct qua nhiều lần poll là đúng cách
-và không rò. Lần điền cuối cùng thì giải phóng bằng `navigation_free_planner_data`.
-
-Ba khác biệt so với bản cũ:
-
-1. **`plan` của `get_local_data` giờ có dữ liệu.** Bản cũ không bao giờ điền trường này cho local.
-2. **`costmap` vẫn được điền đầy đủ** qua bộ xuất costmap — không rỗng.
-3. **`footprint` đổi ý nghĩa.** Bản cũ: polygon **đã pad và đã transform tới pose robot**, `frame_id`
-   là global frame — vẽ thẳng lên bản đồ là đúng chỗ. Bản mới: polygon **thô** host đã set, chưa
-   transform, `frame_id` là `base_link`, và **rỗng nếu host chưa set footprint**.
-
-> Điểm 3 làm vỡ hiển thị footprint. Chỗ sửa đúng nằm ở `move_base2` (lấy footprint từ costmap thay vì
-> từ bản thô) chứ không phải bắt C# tự transform. Xem `C_API_MOVE_BASE2_PLAN.md` mục 8.4. Cho tới khi
-> sửa, C# muốn vẽ đúng thì phải tự xoay/tịnh tiến footprint theo pose lấy từ
-> `navigation_get_robot_pose_2d` — chấp nhận lệch thời gian giữa hai lời gọi.
-
-### 6.11. Bộ nhớ
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `void nav_c_api_free_string(char *)` | = |
-| | `void order_free(Order *)` | = |
-| | `void navigation_free_occupancy_grid(OccupancyGrid *)` | + |
-| | `void navigation_free_laser_scan(LaserScan *)` | + |
-| | `void navigation_free_point_cloud(PointCloud *)` | + |
-| | `void navigation_free_point_cloud2(PointCloud2 *)` | + |
-| | `void navigation_free_planner_data(PlannerDataOutput *)` | + |
-
-Trước đây chỉ có hai hàm đầu, nên grid/scan/cloud/plan **không có hàm giải phóng nào** — mỗi lời gọi
-getter rò một lần. Năm hàm mới đóng lỗ đó. Tất cả đều an toàn với `NULL`, xoá luôn con trỏ và bộ đếm,
-gọi hai lần không sao.
-
-Nguyên tắc: cấp phát ở trong thư viện thì **phải** giải phóng bằng hàm của thư viện. Đừng gọi `free()`
-của libc từ C#.
-
-### 6.12. Tiện ích hình học
-
-| | Hàm | Đổi |
-|---|---|---|
-| | `bool navigation_offset_goal_2d(const Pose2D&, const char *frame_id, double d, PoseStamped &out)` | = |
-| | `bool navigation_offset_goal_stamped(const PoseStamped&, double d, PoseStamped &out)` | = |
-
-Dịch một pose đi `d` mét dọc theo hướng của chính nó (âm là lùi). Thuần tính toán, không đụng lõi.
-(Hai hàm này có export nhưng chưa được khai báo trong `nav_c_api.h`.)
-
---
-
-## 7. Quy tắc bộ nhớ — ai giải phóng cái gì
-
-| Nguồn | Giải phóng bằng |
-|---|---|
-| `NavFeedback.feed_back_str` | `nav_c_api_free_string` |
-| `navigation_get_status_text` | `nav_c_api_free_string` |
-| `navigation_state_to_string` | `nav_c_api_free_string` |
-| `PoseStamped.header.frame_id`, `Twist2DStamped.header.frame_id` từ getter | `nav_c_api_free_string` |
-| `OccupancyGrid` từ `navigation_get_static_map` | `navigation_free_occupancy_grid` |
-| `LaserScan` từ `navigation_get_laser_scan` | `navigation_free_laser_scan` |
-| `PointCloud` / `PointCloud2` từ getter | `navigation_free_point_cloud` / `_point_cloud2` |
-| `PlannerDataOutput` từ `get_global_data` / `get_local_data` | `navigation_free_planner_data` |
-| `Order` từ `convert2COrder` | `order_free` |
-| Mảng truyền **vào** các hàm `add_*` / `move_to_*` | C# tự giữ; thư viện đã copy xong khi hàm trả về |
-
---
-
-## 8. Thread safety
-
-| Nhóm | Gọi được từ thread nào |
-|---|---|
-| `navigation_add_*` (mọi đường sensor) | **Một thread duy nhất.** Đường sensor không thread-safe. Nếu C# có nhiều nguồn chạy song song, dồn qua một hàng đợi rồi bơm từ một thread. |
-| `navigation_set_robot_footprint` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
-| `pause` / `resume` / `cancel` / `set_twist_*` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
-| `move_to*` / `dock_to*` / `rotate_to` / `move_straight_to` | Thread bất kỳ |
-| `get_feedback` / `get_state` / `get_twist` / `get_robot_pose_*` | Thread bất kỳ |
-| `get_global_data` / `get_local_data` | Thread bất kỳ; mỗi consumer nên có **struct riêng**, không dùng chung một `PlannerDataOutput` giữa nhiều timer |
-| `navigation_initialize*` / `navigation_shutdown` / `navigation_destroy` | Thread khởi tạo/thread tắt, **không** song song với nhóm sensor |
-
---
-
-## 9. Chẩn đoán nhanh
-
-| Hiện tượng | Nguyên nhân thường gặp |
-|---|---|
-| `navigation_create` trả `NULL` | Sai `PNKX_NAV_CORE_CONFIG_DIR`, thiếu khoá `MoveBase: library_path`, hoặc `libmove_base2.so` không nằm trong đường tìm |
-| Tiến trình chết ngay trong `initialize` | Chưa bơm static map trước `initialize` — SBPL chờ costmap rồi `exit(1)` |
-| `initialize_checked` trả `false` | Đọc `navigation_get_status_text` — chuỗi nói rõ pha nào hỏng |
-| Gửi goal xong không có gì xảy ra, không log | Control thread chưa chạy: `initialize` đã hỏng nhưng code cũ không kiểm tra kết quả |
-| Robot không tránh vật cản từ camera | Sai tên `topic` khi bơm, hoặc thiếu TF của optical frame, hoặc chưa dùng `navigation_add_depth_camera_data` |
-| Vật cản ma không bao giờ biến mất | Chưa bơm `DepthCameraData` — point cloud không clear được theo frustum |
-| Footprint vẽ sai chỗ trên bản đồ | Mục 6.10 điểm 3 |
-| Bộ nhớ tăng dần | Thiếu `navigation_free_*` ở đường getter, hoặc thiếu `nav_c_api_free_string` cho `feed_back_str` |
-| Tắt chương trình bị treo hoặc crash | Thiếu `navigation_shutdown`, hoặc còn thread bơm sensor chạy khi tắt |
-| Đổi tham số planner mãi không có tác dụng | Sửa nhầm cây config — `move_base2` đọc `Test/move_base2/config/runtime`, không đọc `pnkx_nav_core/config` |
-
---
-
-## 10. Chưa xác nhận
-
- Chưa đọc được mã C# hiện tại, nên chưa biết chỗ nào đang thật sự dựa vào các hành vi cũ ở mục 6.5,
-  6.6 và 6.10.
- Trường `footprint` trong dữ liệu hiển thị: đã đối chiếu cả hai bản và khác biệt là chắc chắn, nhưng
-  chưa xác nhận phía C# có đang vẽ nó lên bản đồ hay không.
- Các hàm mới trong `nav_c_api_2` mới chỉ được kiểm tra biên dịch, **chưa chạy trên robot hay
-  Gazebo**. Đường depth camera cần kiểm tra thực tế bằng bộ đếm chẩn đoán của cổng sensor.
- `navigation_move_straight_to` với khoảng cách âm (lùi) trong `move_base2` chưa được kiểm tra.
-
---
-
-## Phụ lục A — bảng symbol đầy đủ trong `libnav_c_api.so`
-
-Lấy bằng `nm --defined-only` trên các object file. Cột "Nguồn" cho biết symbol nằm trong file nguồn
-nào — chỉ để tra cứu, C# không cần biết: mọi symbol đều nằm chung một `.so`.
-
-### 50 symbol cũ — `nav_c_api.cpp`, C# **không đổi gì**
-
-```
-nav_c_api_free_string                navigation_get_point_cloud2
-navigation_add_laser_scan            navigation_get_robot_footprint
-navigation_add_odometry              navigation_get_robot_pose_2d
-navigation_add_point_cloud           navigation_get_robot_pose_stamped
-navigation_add_point_cloud2          navigation_get_static_map
-navigation_add_static_map            navigation_get_twist
-navigation_cancel                    navigation_initialize
-navigation_create                    navigation_move_straight_to
-navigation_destroy                   navigation_move_to
-navigation_dock_to                   navigation_move_to_nodes_edges
-navigation_dock_to_nodes_edges       navigation_move_to_order
-navigation_dock_to_order             navigation_offset_goal_2d
-navigation_get_all_laser_scans       navigation_offset_goal_stamped
-navigation_get_all_point_cloud2s     navigation_pause
-navigation_get_all_point_clouds      navigation_remove_all_data
-navigation_get_all_static_maps       navigation_remove_all_laser_scans
-navigation_get_feedback              navigation_remove_all_point_cloud2s
-navigation_get_global_data           navigation_remove_all_point_clouds
-navigation_get_laser_scan            navigation_remove_all_static_maps
-navigation_get_local_data            navigation_remove_laser_scan
-navigation_get_point_cloud           navigation_remove_point_cloud
-navigation_resume                    navigation_remove_point_cloud2
-navigation_rotate_to                 navigation_remove_static_map
-navigation_set_robot_footprint       navigation_state_to_string
-navigation_set_twist_angular         navigation_set_twist_linear
-```
-
-Cộng `order_free` từ `convertor.cpp`.
-
-### 13 symbol mới — `nav_c_api_2.cpp`
-
-```
-navigation_initialize_checked        navigation_add_depth_camera_data
-navigation_is_ready                  navigation_move_to_order_v2
-navigation_shutdown                  navigation_dock_to_order_v2
-navigation_get_state                 navigation_free_occupancy_grid
-navigation_get_status_text           navigation_free_laser_scan
-navigation_free_point_cloud          navigation_free_point_cloud2
-navigation_free_planner_data
-```
-
---
-
-## Phụ lục B — phần C# phải thêm
-
-Đây là **toàn bộ** phần khai báo mới. Mọi thứ khác trong lớp binding hiện tại giữ nguyên.
-
-### B.1. Bốn struct mới
-
-Ba struct đầu chưa từng xuất hiện trong bề mặt cũ vì đường depth camera chưa bao giờ được phơi ra.
-
-```csharp
-[StructLayout(LayoutKind.Sequential)]
-public struct RegionOfInterest
-{
-    public uint x_offset;
-    public uint y_offset;
-    public uint height;
-    public uint width;
-    [MarshalAs(UnmanagedType.I1)] public bool do_rectify;
-}
-
-[StructLayout(LayoutKind.Sequential)]
-public struct Image
-{
-    public Header header;
-    public uint   height;
-    public uint   width;
-    public IntPtr encoding;      // char*  — "16UC1" hoặc "32FC1"
-    public byte   is_bigendian;
-    public uint   step;          // số byte mỗi hàng
-    public IntPtr data;          // uint8_t*
-    public UIntPtr data_count;
-}
-
-[StructLayout(LayoutKind.Sequential)]
-public struct CameraInfo
-{
-    public Header  header;
-    public uint    height;
-    public uint    width;
-    public IntPtr  distortion_model;   // char*
-    public IntPtr  D;                  // double*
-    public UIntPtr D_count;
-    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]  public double[] K;
-    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]  public double[] R;
-    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 12)] public double[] P;
-    public uint binning_x;
-    public uint binning_y;
-    public RegionOfInterest roi;
-}
-
-[StructLayout(LayoutKind.Sequential)]
-public struct DepthCameraData
-{
-    public Header     header;        // stamp + optical frame của mẫu
-    public Image      depth;
-    public CameraInfo camera_info;   // intrinsics của ĐÚNG ảnh trên
-}
-```
-
-### B.2. Mười ba `DllImport` mới
-
-```csharp
-private const string Lib = "nav_c_api";
-private const CallingConvention Cdecl = CallingConvention.Cdecl;
-
-// ---- vòng đời ---------------------------------------------------------------------------
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_initialize_checked(IntPtr handle, IntPtr tf3Buffer);
-
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_is_ready(IntPtr handle);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_shutdown(IntPtr handle);
-
-// ---- trạng thái -------------------------------------------------------------------------
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_get_state(IntPtr handle, out NavigationState state);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern IntPtr navigation_get_status_text(IntPtr handle);   // free: nav_c_api_free_string
-
-// ---- sensor -----------------------------------------------------------------------------
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_add_depth_camera_data(
-    IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string topic, DepthCameraData data);
-
-// ---- order ------------------------------------------------------------------------------
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_move_to_order_v2(
-    IntPtr handle,
-    [MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
-    [In] Node[] nodes, UIntPtr nodeCount,
-    [In] Edge[] edges, UIntPtr edgeCount,
-    PoseStamped goal);
-
-[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
-public static extern bool navigation_dock_to_order_v2(
-    IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string marker,
-    [MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
-    [In] Node[] nodes, UIntPtr nodeCount,
-    [In] Edge[] edges, UIntPtr edgeCount,
-    PoseStamped goal);
-
-// ---- giải phóng bộ nhớ ------------------------------------------------------------------
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_free_occupancy_grid(ref OccupancyGrid grid);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_free_laser_scan(ref LaserScan scan);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_free_point_cloud(ref PointCloud cloud);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_free_point_cloud2(ref PointCloud2 cloud);
-
-[DllImport(Lib, CallingConvention = Cdecl)]
-public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
-```
-
-### B.3. Ví dụ bơm một mẫu depth camera
-
-Ảnh depth thường vài MB. Ghim mảng lại thay vì để bộ marshaller sao chép thêm một lần — thư viện đã
-copy sang phía C++ ngay trong lời gọi, nên gỡ ghim ngay sau khi trả về là an toàn.
-
-```csharp
-public bool PushDepthSample(string topic, string opticalFrame,
-                            uint sec, uint nsec,
-                            byte[] depthBytes, uint height, uint width, uint step,
-                            double[] k, double[] p)
-{
-    var frameId   = Marshal.StringToHGlobalAnsi(opticalFrame);
-    var encoding  = Marshal.StringToHGlobalAnsi("16UC1");
-    var distModel = Marshal.StringToHGlobalAnsi("plumb_bob");
-    var pinned    = GCHandle.Alloc(depthBytes, GCHandleType.Pinned);
-    try
-    {
-        var header = new Header { seq = 0, sec = sec, nsec = nsec, frame_id = frameId };
-
-        var data = new DepthCameraData
-        {
-            header = header,
-            depth = new Image
-            {
-                header       = header,
-                height       = height,
-                width        = width,
-                encoding     = encoding,
-                is_bigendian = 0,
-                step         = step,
-                data         = pinned.AddrOfPinnedObject(),
-                data_count   = (UIntPtr)depthBytes.Length,
-            },
-            camera_info = new CameraInfo
-            {
-                header           = header,
-                height           = height,
-                width            = width,
-                distortion_model = distModel,
-                D                = IntPtr.Zero,
-                D_count          = UIntPtr.Zero,
-                K                = k,                 // 9 phần tử
-                R                = new double[9],
-                P                = p,                 // 12 phần tử
-                binning_x        = 0,
-                binning_y        = 0,
-                roi              = default,
-            },
-        };
-
-        return navigation_add_depth_camera_data(navHandle, topic, data);
-    }
-    finally
-    {
-        pinned.Free();
-        Marshal.FreeHGlobal(distModel);
-        Marshal.FreeHGlobal(encoding);
-        Marshal.FreeHGlobal(frameId);
-    }
-}
-```
-
-### B.4. Ví dụ đọc dữ liệu hiển thị mà không rò bộ nhớ
-
-```csharp
-// MỘT struct cho MỖI consumer, giữ lại giữa các lần poll: hàm tự giải phóng nội dung cũ.
-// Đừng dùng chung một struct giữa nhiều timer.
-private PlannerDataOutput globalData;
-
-private void OnGlobalTimer()
-{
-    if (!navigation_get_global_data(navHandle, ref globalData))
-        return;
-
-    DrawPlan(globalData.plan);
-    DrawCostmap(globalData.costmap, globalData.is_costmap_updated, globalData.costmap_update);
-}
-
-// Khi dừng consumer — nếu không, lần điền cuối cùng sẽ rò.
-private void StopGlobalTimer()
-{
-    timer.Stop();
-    navigation_free_planner_data(ref globalData);
-}
-```
--- a/src/APIs/c_api/include/convertor_2.h
+++ b/src/APIs/c_api/include/convertor_2.h
@@ -1,51 +0,0 @@
-#ifndef C_API_CONVERTOR_2_H
-#define C_API_CONVERTOR_2_H
-
-/**
- * @file convertor_2.h
- * @brief Converters that only the move_base2 entry points need.
- *
- * Kept apart from convertor.h so the move_base2 surface can be added, reviewed and reverted without
- * touching the converters the existing binding already depends on.
- */
-
-// C
-#include "std_msgs/Header.h"
-#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CameraInfo.h"
-#include "sensor_msgs/DepthCameraData.h"
-
-// C++
-#include <robot_std_msgs/Header.h>
-#include <robot_sensor_msgs/Image.h>
-#include <robot_sensor_msgs/CameraInfo.h>
-#include <robot_sensor_msgs/DepthCameraData.h>
-
-/**
- * @brief Convert C Header to C++ Header.
- * @param header C header; `frame_id` may be NULL (becomes an empty string).
- */
-robot_std_msgs::Header convert2CppHeader(const Header& header);
-
-/**
- * @brief Convert C Image to C++ Image.
- *
- * The pixel buffer is copied, so the caller keeps ownership of `image.data` and may free it as
- * soon as this call returns.
- *
- * @param image C image; `data` and `encoding` may be NULL.
- */
-robot_sensor_msgs::Image convert2CppImage(const Image& image);
-
-/**
- * @brief Convert C CameraInfo to C++ CameraInfo.
- * @param info C camera info; `D` and `distortion_model` may be NULL. K/R/P are fixed size (9/9/12).
- */
-robot_sensor_msgs::CameraInfo convert2CppCameraInfo(const CameraInfo& info);
-
-/**
- * @brief Convert C DepthCameraData to C++ DepthCameraData. Everything is deep-copied.
- */
-robot_sensor_msgs::DepthCameraData convert2CppDepthCameraData(const DepthCameraData& data);
-
-#endif // C_API_CONVERTOR_2_H
--- a/src/APIs/c_api/include/nav_c_api_2.h
+++ b/src/APIs/c_api/include/nav_c_api_2.h
@@ -1,301 +0,0 @@
-#ifndef NAVIGATION_C_API_2_H
-#define NAVIGATION_C_API_2_H
-
-/**
- * @file nav_c_api_2.h
- * @brief Additional C entry points required by the move_base2 navigation runtime.
- *
- * ## What this file is
- *
- * `nav_c_api.h` covers the navigation contract as the previous runtime exposed it. Every one of
- * those functions keeps working against move_base2 — move_base2 implements
- * `robot::move_base_core::BaseNavigation` with the signatures unchanged, so no existing binding
- * breaks. This file adds what that contract gained but the C surface never exposed, plus the two
- * entry points move_base2 makes newly necessary:
- *
- *  - navigation_shutdown()               — quiesce the core before destroying it.
- *  - navigation_is_ready()               — did initialize() actually succeed?
- *  - navigation_initialize_checked()     — initialize + verify, in one call.
- *  - navigation_get_state()              — poll the state without allocating a string.
- *  - navigation_get_status_text()        — the core's own explanation, for logging.
- *  - navigation_add_depth_camera_data()  — the depth-camera input path, never exposed before.
- *  - navigation_move_to_order_v2()       — an order carrying a real orderId / orderUpdateId.
- *  - navigation_dock_to_order_v2()       — same, for docking.
- *
- * Include this header **in addition to** nav_c_api.h. It adds symbols; it replaces none.
- *
- * ## What changed underneath the functions you already call
- *
- * These keep their signatures and their meaning, but behave differently enough to matter:
- *
- *  1. navigation_initialize() is now heavy and blocking — it builds both costmaps, loads the
- *     planners, the controller, the recovery behaviours and the mission layer, then starts the
- *     control thread. It still returns `true` unconditionally because the underlying contract
- *     returns void. Use navigation_initialize_checked(), or call navigation_is_ready() after it.
- *
- *  2. A static map must be pushed **before** initialize(). The lattice planner blocks until the
- *     costmap has a non-zero size, and that size only exists once a static map has reached the
- *     static layer. It aborts the whole process if it waits too long.
- *
- *  3. navigation_set_robot_footprint() may now be called before initialize() — the footprint is
- *     cached and applied while the costmaps are built. Under the old runtime the same call was
- *     dropped silently because the costmaps did not exist yet.
- *
- *  4. navigation_pause() / _resume() / _cancel() no longer take effect inside the call. They record
- *     the request; the control thread applies it on its next cycle. Returning from the call does
- *     not mean the robot stopped — watch the state, or watch the twist.
- *
- *  5. navigation_set_twist_linear() / _angular() set a velocity **ceiling**, and the sign of the
- *     linear x component selects which direction the ceiling applies to. They now only reject
- *     NaN/Inf; the old runtime also returned false while a cancel was in flight, and that signal no
- *     longer exists. The new ceiling takes effect on the next control cycle.
- *
- *  6. navigation_get_twist() returns the command being issued, not a measurement. While no request
- *     is running its stamp stops advancing; a zero stamp means the core has never commanded
- *     anything. Check the stamp before republishing it as a velocity command.
- *
- *  7. NavFeedback.feed_back_str now carries the core's own reason strings — different text from the
- *     old runtime. Branch on NavigationState, never on the string.
- *
- *  8. NavFeedback.navigation_state deliberately reports ACTIVE between the legs of a multi-leg
- *     order, where the core itself is momentarily SUCCEEDED or PENDING. Treating SUCCEEDED as "the
- *     order finished" stays correct only because of that; do not bypass it.
- *
- *  9. navigation_add_odometry() feeds the controller's measured velocity every cycle, not just a
- *     stored value. Push it at the rate the odometry source produces it.
- *
- * 10. The `footprint` field of navigation_get_global_data() / _local_data() is the raw footprint the
- *     host set, in the robot base frame, and it is empty if the host never set one. The old runtime
- *     returned the padded footprint already transformed to the robot pose in the global frame.
- *
- * 11. navigation_destroy() alone is no longer enough — call navigation_shutdown() first, from a
- *     point in your own shutdown sequence where the process is still fully alive.
- *
- * See NAV_C_API_2_GUIDE.md in this directory for the per-function walkthrough and the startup and
- * shutdown sequences.
- */
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <stddef.h>
-
-#include "nav_c_api.h"
-#include "sensor_msgs/DepthCameraData.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/PointCloud.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "protocol_msgs/Node.h"
-#include "protocol_msgs/Edge.h"
-
-  // ============================================================================================
-  // Lifecycle
-  // ============================================================================================
-
-  /**
-   * @brief Initialize the navigation core and report whether it actually came up.
-   *
-   * Wraps navigation_initialize(). The underlying contract method returns void, so every failure
-   * path inside the runtime — costmap construction, sensor configuration, plugin loading, control
-   * loop configuration, control thread startup — returns normally and only records the reason in
-   * the feedback. This function reads that back and turns it into a return value.
-   *
-   * Blocking and expensive: it builds two costmaps, loads every plugin named in the YAML, starts
-   * the map-update threads, the mission threads and the control thread. Do not call it from a UI
-   * thread.
-   *
-   * @param handle       Navigation handle from navigation_create().
-   * @param tf3_buffer   tf3 BufferCore the host owns; must outlive the navigation instance.
-   * @return true only when the core is ready to accept goals.
-   *
-   * @pre At least one static map has been pushed with navigation_add_static_map().
-   * @pre The robot footprint has been set with navigation_set_robot_footprint(), if the host wants
-   *      its own footprint rather than the one in the YAML.
-   * @see navigation_get_status_text() for the reason behind a false return.
-   */
-  bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer);
-
-  /**
-   * @brief Is the core initialized and accepting goals?
-   *
-   * Cheap enough to poll. Reads the same flag navigation_initialize_checked() checks, without
-   * allocating the feedback string.
-   *
-   * @return false if the handle is null, the core never initialized, or initialization failed.
-   */
-  bool navigation_is_ready(NavigationHandle handle);
-
-  /**
-   * @brief Stop the core's threads without destroying the instance.
-   *
-   * Gives the host a point inside its own shutdown sequence — while the process is still fully
-   * alive — to stop the control thread, the costmap update threads and the mission threads.
-   * Destroying the instance from a finalizer, after the runtime has already begun tearing down, is
-   * the race this exists to avoid.
-   *
-   * After this call the core issues no further velocity commands, and every getter
-   * (navigation_get_feedback, navigation_get_twist, navigation_get_robot_pose_*) remains safe to
-   * call. Safe to call more than once.
-   *
-   * @warning No thread may be calling navigation_add_* while this runs, and none may call them
-   *          afterwards. The sensor path is not thread-safe and there is no longer a control thread
-   *          consuming what it produces.
-   */
-  void navigation_shutdown(NavigationHandle handle);
-
-  // ============================================================================================
-  // Status
-  // ============================================================================================
-
-  /**
-   * @brief Current navigation state, without allocating anything.
-   *
-   * navigation_get_feedback() strdup()s its message on every call, so polling it at control rate
-   * means one allocation and one free per poll — and one leak per missed free. Use this when only
-   * the state matters, which is the common case.
-   *
-   * @param handle    Navigation handle.
-   * @param out_state Receives the current state; untouched on failure.
-   * @return false if the handle is null or the core has no feedback yet.
-   */
-  bool navigation_get_state(NavigationHandle handle, NavigationState *out_state);
-
-  /**
-   * @brief The core's own description of what it is doing, or why it refused.
-   *
-   * For logging and diagnostics only. The wording comes from the runtime and changes between
-   * versions — never branch on it. Branch on navigation_get_state().
-   *
-   * @return Newly allocated string, or NULL. Free with nav_c_api_free_string().
-   */
-  char *navigation_get_status_text(NavigationHandle handle);
-
-  // ============================================================================================
-  // Sensor input
-  // ============================================================================================
-
-  /**
-   * @brief Push one coherent depth-camera sample into the navigation core.
-   *
-   * This is the only way to reach the depth-camera path from C. The voxel layer projects the depth
-   * image through the intrinsics to build the camera frustum, which is what lets it *clear* stale
-   * obstacles inside the field of view. A point cloud cannot substitute: it carries no intrinsics,
-   * so no frustum can be reconstructed and nothing gets cleared.
-   *
-   * @param handle Navigation handle.
-   * @param topic  Logical name of the stream. Must match an `observation_source` declared for the
-   *               voxel layer in the costmap YAML — a name no layer subscribes to is accepted here
-   *               and then dropped, silently, further down.
-   * @param data   Depth image and its matching intrinsics. Deep-copied; the caller may release its
-   *               buffers as soon as this returns.
-   * @return true if the sample was accepted.
-   *
-   * @pre `data.header.frame_id` is the camera's optical frame, and a transform from it to the robot
-   *      base frame exists in the tf3 buffer at `data.header.stamp`.
-   * @pre `data.depth.encoding` matches the real encoding ("16UC1" or "32FC1") and `step` is the
-   *      real row stride in bytes.
-   * @pre `data.camera_info` describes the resolution actually being sent, not the colour stream's.
-   * @pre The depth image and the camera info come from the same instant.
-   *
-   * @note Push at the camera's real rate. This is the most expensive sensor path in the core;
-   *       pushing faster than the source only burns CPU.
-   */
-  bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
-                                        const DepthCameraData data);
-
-  // ============================================================================================
-  // Orders
-  // ============================================================================================
-
-  /**
-   * @brief Send a multi-node order with a real identity.
-   *
-   * Under move_base2 an order goes through the mission layer, which splits it into legs at the
-   * nodes that carry actions, filters by `released`, and can extend the route when the fleet master
-   * releases more of the horizon. `orderId` plus `orderUpdateId` is what identifies an order there:
-   * the same `orderId` with a higher `orderUpdateId` is an update of the route in progress, while a
-   * different `orderId` is a new order that replaces it.
-   *
-   * navigation_move_to_nodes_edges() cannot express that — it has nowhere to put either field — so
-   * every call looks like a different order and no update can ever be sent. Use this instead
-   * whenever the fleet master sends order updates or releases the horizon progressively.
-   *
-   * @param handle           Navigation handle.
-   * @param order_id         VDA5050 orderId. Must not be NULL or empty.
-   * @param order_update_id  VDA5050 orderUpdateId. Must increase within one orderId.
-   * @param nodes            Node array; the host keeps ownership.
-   * @param node_count       Number of nodes.
-   * @param edges            Edge array; the host keeps ownership.
-   * @param edge_count       Number of edges.
-   * @param goal             Final pose, in the global frame.
-   * @return true if the order was accepted.
-   */
-  bool navigation_move_to_order_v2(NavigationHandle handle,
-                                   const char *order_id, uint32_t order_update_id,
-                                   const Node *nodes, size_t node_count,
-                                   const Edge *edges, size_t edge_count,
-                                   const PoseStamped goal);
-
-  /**
-   * @brief Docking counterpart of navigation_move_to_order_v2().
-   *
-   * @param handle           Navigation handle.
-   * @param marker           Docking marker name or id. Must not be NULL.
-   * @param order_id         VDA5050 orderId. Must not be NULL or empty.
-   * @param order_update_id  VDA5050 orderUpdateId.
-   * @param nodes            Node array; the host keeps ownership.
-   * @param node_count       Number of nodes.
-   * @param edges            Edge array; the host keeps ownership.
-   * @param edge_count       Number of edges.
-   * @param goal             Docking pose.
-   * @return true if the docking order was accepted.
-   */
-  bool navigation_dock_to_order_v2(NavigationHandle handle, const char *marker,
-                                   const char *order_id, uint32_t order_update_id,
-                                   const Node *nodes, size_t node_count,
-                                   const Edge *edges, size_t edge_count,
-                                   const PoseStamped goal);
-
-  // ============================================================================================
-  // Releasing what the getters allocate
-  // ============================================================================================
-  //
-  // Every navigation_get_* function that returns a grid, a scan, a cloud or a plan fills it with
-  // buffers allocated inside the library. Only two release helpers ever shipped —
-  // nav_c_api_free_string() and order_free() — so the rest had no counterpart and leaked one
-  // allocation per call. At four display timers polling at 10 Hz that is a steady leak, not a
-  // rounding error. These close it.
-  //
-  // All are null-safe, clear the pointers and counts they release, and are safe to call twice.
-
-  /** @brief Release the buffers inside an OccupancyGrid filled by navigation_get_static_map(). */
-  void navigation_free_occupancy_grid(OccupancyGrid *grid);
-
-  /** @brief Release the buffers inside a LaserScan filled by navigation_get_laser_scan(). */
-  void navigation_free_laser_scan(LaserScan *scan);
-
-  /** @brief Release the buffers inside a PointCloud filled by navigation_get_point_cloud(). */
-  void navigation_free_point_cloud(PointCloud *cloud);
-
-  /** @brief Release the buffers inside a PointCloud2 filled by navigation_get_point_cloud2(). */
-  void navigation_free_point_cloud2(PointCloud2 *cloud);
-
-  /**
-   * @brief Release the buffers inside a PlannerDataOutput.
-   *
-   * navigation_get_global_data() / _local_data() already release the previous contents of the
-   * struct they are handed, so reusing one struct across polls does not leak. What does leak is the
-   * last fill, once the host stops polling — free it here.
-   */
-  void navigation_free_planner_data(PlannerDataOutput *data);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // NAVIGATION_C_API_2_H
--- a/src/APIs/c_api/include/sensor_msgs/DepthCameraData.h
+++ b/src/APIs/c_api/include/sensor_msgs/DepthCameraData.h
@@ -1,36 +0,0 @@
-#ifndef C_API_SENSOR_MSGS_DEPTHCAMERADATA_H
-#define C_API_SENSOR_MSGS_DEPTHCAMERADATA_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>
-#include <stddef.h>
-#include "std_msgs/Header.h"
-#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CameraInfo.h"
-
-/**
- * @brief One coherent depth-camera sample: a depth image plus the intrinsics that describe it.
- *
- * Mirrors robot_sensor_msgs::DepthCameraData. "Coherent" is the whole point of the struct: the
- * voxel layer projects every depth pixel through the intrinsics to build a frustum, so a depth
- * image paired with intrinsics from a different resolution or a different instant clears the wrong
- * volume of space. Pair them at the source, not here.
- */
-typedef struct
-{
-  /** Canonical timestamp and optical frame of this sample. */
-  Header header;
-  /** Depth image. `encoding` must be the true encoding ("16UC1" or "32FC1"). */
-  Image depth;
-  /** Intrinsics of the exact image above — same resolution, same instant. */
-  CameraInfo camera_info;
-} DepthCameraData;
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // C_API_SENSOR_MSGS_DEPTHCAMERADATA_H
--- a/src/APIs/c_api/src/convertor_2.cpp
+++ b/src/APIs/c_api/src/convertor_2.cpp
@@ -1,78 +0,0 @@
-#include "convertor_2.h"
-
-#include <algorithm>
-#include <cstddef>
-#include <string>
-
-namespace
-{
-
-/// Empty string for a NULL C string — every char* coming from the host is optional.
-inline std::string safe_string(const char* s)
-{
-  return s != nullptr ? std::string(s) : std::string();
-}
-
-}  // namespace
-
-robot_std_msgs::Header convert2CppHeader(const Header& header)
-{
-  robot_std_msgs::Header cpp;
-  cpp.seq = header.seq;
-  cpp.stamp.sec = header.sec;
-  cpp.stamp.nsec = header.nsec;
-  cpp.frame_id = safe_string(header.frame_id);
-  return cpp;
-}
-
-robot_sensor_msgs::Image convert2CppImage(const Image& image)
-{
-  robot_sensor_msgs::Image cpp;
-  cpp.header = convert2CppHeader(image.header);
-  cpp.height = image.height;
-  cpp.width = image.width;
-  cpp.encoding = safe_string(image.encoding);
-  cpp.is_bigendian = image.is_bigendian;
-  cpp.step = image.step;
-
-  // Copy, never alias: the host buffer may be a pinned .NET array released right after this call,
-  // while the voxel layer reads the sample later on the map-update thread.
-  const size_t count = image.data != nullptr ? image.data_count : 0;
-  cpp.data.assign(image.data, image.data + count);
-  return cpp;
-}
-
-robot_sensor_msgs::CameraInfo convert2CppCameraInfo(const CameraInfo& info)
-{
-  robot_sensor_msgs::CameraInfo cpp;
-  cpp.header = convert2CppHeader(info.header);
-  cpp.height = info.height;
-  cpp.width = info.width;
-  cpp.distortion_model = safe_string(info.distortion_model);
-
-  const size_t d_count = info.D != nullptr ? info.D_count : 0;
-  cpp.D.assign(info.D, info.D + d_count);
-
-  std::copy(info.K, info.K + 9, cpp.K.begin());
-  std::copy(info.R, info.R + 9, cpp.R.begin());
-  std::copy(info.P, info.P + 12, cpp.P.begin());
-
-  cpp.binning_x = info.binning_x;
-  cpp.binning_y = info.binning_y;
-
-  cpp.roi.x_offset = info.roi.x_offset;
-  cpp.roi.y_offset = info.roi.y_offset;
-  cpp.roi.height = info.roi.height;
-  cpp.roi.width = info.roi.width;
-  cpp.roi.do_rectify = info.roi.do_rectify;
-  return cpp;
-}
-
-robot_sensor_msgs::DepthCameraData convert2CppDepthCameraData(const DepthCameraData& data)
-{
-  robot_sensor_msgs::DepthCameraData cpp;
-  cpp.header = convert2CppHeader(data.header);
-  cpp.depth = convert2CppImage(data.depth);
-  cpp.camera_info = convert2CppCameraInfo(data.camera_info);
-  return cpp;
-}
--- a/src/APIs/c_api/src/nav_c_api.cpp
+++ b/src/APIs/c_api/src/nav_c_api.cpp
@@ -317,9 +317,7 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
    if (!nav_ptr)
      return false;
    robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
-    // Zero-init: the fields left unset below are all char*, and convert2CppOrder treats any
-    // non-null pointer as a valid string — an uninitialized struct hands it stack garbage.
-    Order order{};
+    Order order;
    order.nodes = const_cast<Node *>(nodes);
    order.nodes_count = node_count;
    order.edges = const_cast<Edge *>(edges);
@@ -418,8 +416,7 @@ extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const ch
            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
    if (!nav_ptr)
      return false;
-    // Zero-init: see navigation_move_to_nodes_edges.
-    Order order{};
+    Order order;
    order.nodes = const_cast<Node *>(nodes);
    order.nodes_count = node_count;
    order.edges = const_cast<Edge *>(edges);
@@ -572,16 +569,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/APIs/c_api/src/nav_c_api_2.cpp
+++ b/src/APIs/c_api/src/nav_c_api_2.cpp
@@ -1,422 +0,0 @@
-/**
- * @file nav_c_api_2.cpp
- * @brief Implementation of the move_base2 additions to the C navigation API.
- *
- * Deliberately separate from nav_c_api.cpp: the surface the existing binding depends on stays byte
- * for byte where it is, and everything move_base2 needs on top of it lands here, where it can be
- * reviewed — or dropped — on its own.
- *
- * The handle is the same opaque `robot::move_base_core::BaseNavigation*` navigation_create()
- * returns; ownership still lives in nav_c_api.cpp, so nothing here creates or frees an instance.
- */
-
-#include "nav_c_api_2.h"
-
-#include <cstdlib>
-#include <cstring>
-#include <string>
-
-#include <boost/make_shared.hpp>
-
-#include <robot/robot.h>
-#include <move_base_core/common.h>
-#include <move_base_core/navigation.h>
-#include <robot_sensor_msgs/DepthCameraData.h>
-
-#include "convertor.h"
-#include "convertor_2.h"
-
-namespace
-{
-
-/// Non-owning view of the handle. The instance is owned by the table in nav_c_api.cpp.
-inline robot::move_base_core::BaseNavigation *as_navigation(NavigationHandle handle)
-{
-  return static_cast<robot::move_base_core::BaseNavigation *>(handle);
-}
-
-/**
- * @brief Build a C Order from an id plus the node/edge arrays the host owns.
- *
- * Zero-initialized on purpose. Every optional field of Order is a char*, and the converter treats a
- * non-null pointer as a valid string — an uninitialized struct therefore hands it stack garbage to
- * build std::string from. The pointers below stay non-owning: the converter copies before this
- * returns, and the arrays are never freed here.
- */
-Order make_order(const char *order_id, uint32_t order_update_id,
-                 const Node *nodes, size_t node_count,
-                 const Edge *edges, size_t edge_count)
-{
-  Order order{};
-  order.orderId = const_cast<char *>(order_id);
-  order.orderUpdateId = order_update_id;
-  order.nodes = const_cast<Node *>(nodes);
-  order.nodes_count = nodes != nullptr ? node_count : 0;
-  order.edges = const_cast<Edge *>(edges);
-  order.edges_count = edges != nullptr ? edge_count : 0;
-  return order;
-}
-
-}  // namespace
-
-// ================================================================================================
-// Lifecycle
-// ================================================================================================
-
-extern "C" bool navigation_initialize_checked(NavigationHandle handle, TFListenerHandle tf3_buffer)
-{
-  if (!handle || !tf3_buffer)
-  {
-    robot::log_error("navigation_initialize_checked: null handle or tf3 buffer");
-    return false;
-  }
-
-  try
-  {
-    auto *nav = as_navigation(handle);
-
-    // The host owns the buffer and must outlive the core; hand over a non-owning alias.
-    robot::TFListenerPtr tf(static_cast<tf3::BufferCore *>(tf3_buffer),
-                            [](tf3::BufferCore *) {});
-
-    nav->initialize(tf);
-
-    // initialize() returns void, so the runtime reports failure the only way it can: it leaves
-    // is_ready false and writes the reason into the feedback. Not reading it back is how a host
-    // ends up pushing goals into a core that has no control thread.
-    robot::move_base_core::NavFeedback *feedback = nav->getFeedback();
-    if (feedback == nullptr)
-    {
-      robot::log_error("navigation_initialize_checked: core exposes no feedback");
-      return false;
-    }
-    if (!feedback->is_ready)
-    {
-      robot::log_error("navigation_initialize_checked: core not ready: %s",
-                       feedback->feed_back_str.c_str());
-      return false;
-    }
-    return true;
-  }
-  catch (const std::exception &e)
-  {
-    robot::log_error("navigation_initialize_checked failed: %s", e.what());
-    return false;
-  }
-  catch (...)
-  {
-    robot::log_error("navigation_initialize_checked failed: unknown exception");
-    return false;
-  }
-}
-
-extern "C" bool navigation_is_ready(NavigationHandle handle)
-{
-  if (!handle)
-  {
-    return false;
-  }
-
-  try
-  {
-    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
-    return feedback != nullptr && feedback->is_ready;
-  }
-  catch (...)
-  {
-    return false;
-  }
-}
-
-extern "C" void navigation_shutdown(NavigationHandle handle)
-{
-  if (!handle)
-  {
-    return;
-  }
-
-  try
-  {
-    as_navigation(handle)->shutdown();
-  }
-  catch (const std::exception &e)
-  {
-    // Swallowed on purpose: this runs inside the host's shutdown sequence, where throwing across
-    // the C boundary would abort a process that is already on its way down.
-    robot::log_error("navigation_shutdown failed: %s", e.what());
-  }
-  catch (...)
-  {
-    robot::log_error("navigation_shutdown failed: unknown exception");
-  }
-}
-
-// ================================================================================================
-// Status
-// ================================================================================================
-
-extern "C" bool navigation_get_state(NavigationHandle handle, NavigationState *out_state)
-{
-  if (!handle || !out_state)
-  {
-    return false;
-  }
-
-  try
-  {
-    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
-    if (feedback == nullptr)
-    {
-      return false;
-    }
-    *out_state = static_cast<NavigationState>(static_cast<int>(feedback->navigation_state));
-    return true;
-  }
-  catch (...)
-  {
-    return false;
-  }
-}
-
-extern "C" char *navigation_get_status_text(NavigationHandle handle)
-{
-  if (!handle)
-  {
-    return nullptr;
-  }
-
-  try
-  {
-    robot::move_base_core::NavFeedback *feedback = as_navigation(handle)->getFeedback();
-    if (feedback == nullptr || feedback->feed_back_str.empty())
-    {
-      return nullptr;
-    }
-    return strdup(feedback->feed_back_str.c_str());
-  }
-  catch (...)
-  {
-    return nullptr;
-  }
-}
-
-// ================================================================================================
-// Sensor input
-// ================================================================================================
-
-extern "C" bool navigation_add_depth_camera_data(NavigationHandle handle, const char *topic,
-                                                 const DepthCameraData data)
-{
-  if (!handle || !topic)
-  {
-    return false;
-  }
-
-  try
-  {
-    // ConstPtr is a boost::shared_ptr and the core keeps the sample for as long as it needs it, so
-    // hand it a copy rather than a view of host memory.
-    auto sample = boost::make_shared<robot_sensor_msgs::DepthCameraData>(
-        convert2CppDepthCameraData(data));
-
-    as_navigation(handle)->addDepthCameraData(std::string(topic), sample);
-    return true;
-  }
-  catch (const std::exception &e)
-  {
-    robot::log_error("navigation_add_depth_camera_data failed: %s", e.what());
-    return false;
-  }
-  catch (...)
-  {
-    return false;
-  }
-}
-
-// ================================================================================================
-// Orders
-// ================================================================================================
-
-extern "C" bool navigation_move_to_order_v2(NavigationHandle handle,
-                                            const char *order_id, uint32_t order_update_id,
-                                            const Node *nodes, size_t node_count,
-                                            const Edge *edges, size_t edge_count,
-                                            const PoseStamped goal)
-{
-  if (!handle || order_id == nullptr || order_id[0] == '\0')
-  {
-    robot::log_error("navigation_move_to_order_v2: null handle or empty orderId");
-    return false;
-  }
-  if (!isQuaternionValid(goal.pose.orientation))
-  {
-    robot::log_error("navigation_move_to_order_v2: goal quaternion is invalid");
-    return false;
-  }
-
-  try
-  {
-    const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
-    return as_navigation(handle)->moveTo(convert2CppOrder(order), convert2CppPoseStamped(goal));
-  }
-  catch (const std::exception &e)
-  {
-    robot::log_error("navigation_move_to_order_v2 failed: %s", e.what());
-    return false;
-  }
-  catch (...)
-  {
-    return false;
-  }
-}
-
-extern "C" bool navigation_dock_to_order_v2(NavigationHandle handle, const char *marker,
-                                            const char *order_id, uint32_t order_update_id,
-                                            const Node *nodes, size_t node_count,
-                                            const Edge *edges, size_t edge_count,
-                                            const PoseStamped goal)
-{
-  if (!handle || !marker || order_id == nullptr || order_id[0] == '\0')
-  {
-    robot::log_error("navigation_dock_to_order_v2: null handle, marker or empty orderId");
-    return false;
-  }
-  if (!isQuaternionValid(goal.pose.orientation))
-  {
-    robot::log_error("navigation_dock_to_order_v2: goal quaternion is invalid");
-    return false;
-  }
-
-  try
-  {
-    const Order order = make_order(order_id, order_update_id, nodes, node_count, edges, edge_count);
-    return as_navigation(handle)->dockTo(convert2CppOrder(order), std::string(marker),
-                                         convert2CppPoseStamped(goal));
-  }
-  catch (const std::exception &e)
-  {
-    robot::log_error("navigation_dock_to_order_v2 failed: %s", e.what());
-    return false;
-  }
-  catch (...)
-  {
-    return false;
-  }
-}
-
-// ================================================================================================
-// Releasing what the getters allocate
-// ================================================================================================
-//
-// The converters allocate with malloc()/strdup(), so these release with free(). Mixing in delete[]
-// here would be undefined behaviour, and the host cannot free these itself on Windows-style
-// runtimes anyway — allocation and release must happen inside the same C runtime.
-
-namespace
-{
-
-/// free() a buffer and clear both the pointer and its element count in one step.
-template <typename T>
-inline void free_array(T *&ptr, size_t &count)
-{
-  free(ptr);
-  ptr = nullptr;
-  count = 0;
-}
-
-inline void free_string_field(char *&str)
-{
-  free(str);
-  str = nullptr;
-}
-
-}  // namespace
-
-extern "C" void navigation_free_occupancy_grid(OccupancyGrid *grid)
-{
-  if (!grid)
-  {
-    return;
-  }
-  free_string_field(grid->header.frame_id);
-  free_array(grid->data, grid->data_count);
-}
-
-extern "C" void navigation_free_laser_scan(LaserScan *scan)
-{
-  if (!scan)
-  {
-    return;
-  }
-  free_string_field(scan->header.frame_id);
-  free_array(scan->ranges, scan->ranges_count);
-  free_array(scan->intensities, scan->intensities_count);
-}
-
-extern "C" void navigation_free_point_cloud(PointCloud *cloud)
-{
-  if (!cloud)
-  {
-    return;
-  }
-  free_string_field(cloud->header.frame_id);
-  free_array(cloud->points, cloud->points_count);
-
-  // Each channel owns a name and a value buffer of its own; freeing the array alone leaks both.
-  if (cloud->channels != nullptr)
-  {
-    for (size_t i = 0; i < cloud->channels_count; ++i)
-    {
-      free_string_field(cloud->channels[i].name);
-      free_array(cloud->channels[i].values, cloud->channels[i].values_count);
-    }
-  }
-  free_array(cloud->channels, cloud->channels_count);
-}
-
-extern "C" void navigation_free_point_cloud2(PointCloud2 *cloud)
-{
-  if (!cloud)
-  {
-    return;
-  }
-  free_string_field(cloud->header.frame_id);
-
-  if (cloud->fields != nullptr)
-  {
-    for (size_t i = 0; i < cloud->fields_count; ++i)
-    {
-      free_string_field(cloud->fields[i].name);
-    }
-  }
-  free_array(cloud->fields, cloud->fields_count);
-  free_array(cloud->data, cloud->data_count);
-}
-
-extern "C" void navigation_free_planner_data(PlannerDataOutput *data)
-{
-  if (!data)
-  {
-    return;
-  }
-
-  if (data->plan.poses != nullptr)
-  {
-    for (size_t i = 0; i < data->plan.poses_count; ++i)
-    {
-      free_string_field(data->plan.poses[i].header.frame_id);
-    }
-  }
-  free_array(data->plan.poses, data->plan.poses_count);
-  free_string_field(data->plan.header.frame_id);
-
-  navigation_free_occupancy_grid(&data->costmap);
-
-  free_string_field(data->costmap_update.header.frame_id);
-  free_array(data->costmap_update.data, data->costmap_update.data_count);
-
-  free_string_field(data->footprint.header.frame_id);
-  free_array(data->footprint.polygon.points, data->footprint.polygon.points_count);
-
-  data->is_costmap_updated = false;
-}
--- 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/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
--- 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
@@ -368,7 +368,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 +404,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 +477,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,12 +512,8 @@ 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; 
+  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_))
@@ -540,23 +528,27 @@ 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
      this->computePurePursuit(
      carrot_pose, 
@@ -567,7 +559,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_)
    {
@@ -581,7 +573,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
      result.velocity = drive_cmd;
      return result;
    }
-    
  }
  result.poses.clear();
  result.poses.reserve(transformed_plan.poses.size());
@@ -594,12 +585,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;
@@ -625,15 +610,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,8 +629,9 @@ 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;
+  double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa);
  if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
  {
    if (trajectory.poses.size() >= 2) {
@@ -654,20 +640,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 +664,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,7 +683,8 @@ 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;

@@ -719,8 +703,7 @@ 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], -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);
+    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
 }

 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -744,9 +727,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 +761,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)
@@ -815,13 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
  //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;

  bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-#ifdef BUILD_WITH_ROS
-  if (result)
-    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-#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
+// #ifdef BUILD_WITH_ROS
+  // if (result)
+  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+
+  // else if(fabs(velocity.x) < min_speed_xy_)
+  // {
+  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
+  //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  // }
+// #endif
  return result;
 }

@@ -903,11 +880,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 +936,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 +1009,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 +1191,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 +1203,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 +1221,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 +1229,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 +1276,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 +1343,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 +1406,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/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
@@ -45,8 +45,6 @@ namespace two_points_planner

      name_ = name;
      costmap_robot_ = costmap_robot;
-
-      
      
      if(!costmap_robot_ || !costmap_robot_->getCostmap()) 
      {
@@ -57,8 +55,6 @@ 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;
@@ -123,18 +119,6 @@ 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);
@@ -192,9 +176,7 @@ namespace two_points_planner
    const double dy = goal.pose.position.y - start.pose.position.y;
    const double distance = std::sqrt(dx * dx + dy * dy);
    double theta;
-    // Lấy độ phân giải của costmap
-    double resolution = costmap_robot_->getCostmap()->getResolution();
-    
+
    if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
    {
      theta = std::atan2(dy, dx);
@@ -207,12 +189,16 @@ 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);
+      pose.pose.position.x += 0.01 * cos(theta);
+      pose.pose.position.y += 0.01 * sin(theta);
      plan.push_back(pose);
      plan.push_back(goal);
      return true;
    }
+
+    // Lấy độ phân giải của costmap
+    double resolution = costmap_robot_->getCostmap()->getResolution();
+
    // Tính số điểm cần chia
    int num_points = std::ceil(distance / resolution);

--- 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
@@ -250,12 +250,17 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
  if(rotate_algorithm_) rotate_algorithm_->reset();
  ret_nav_ = ret_angle_ = false;

+  robot::log_info_at(__FILE__, __LINE__, "Debug");
+  parent_.printParams();
  std::string algorithm_nav_name;
  planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));

  parent_.setParam(algorithm_nav_name, original_papams_);
  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
  nh_algorithm.setParam("allow_rotate", false);
+
+  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
+  parent_.printParams();
 }

 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -366,7 +371,6 @@ 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
  {
@@ -425,9 +429,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);
@@ -451,6 +455,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);
    }
  }

@@ -504,7 +509,6 @@ 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;
@@ -526,7 +530,6 @@ 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;
@@ -798,8 +801,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_local_planner.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
@@ -208,14 +208,12 @@ 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;
--- a/src/Libraries/common_msgs
+++ b/src/Libraries/common_msgs
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
--- a/src/Libraries/costmap_2d.zip
+++ b/src/Libraries/costmap_2d.zip
--- a/src/Libraries/costmap_2d_dev_1_cam.zip
+++ b/src/Libraries/costmap_2d_dev_1_cam.zip
--- a/src/Libraries/costmap_2d_dev_multi_cam_test.zip
+++ b/src/Libraries/costmap_2d_dev_multi_cam_test.zip
--- a/src/Libraries/data_convert
+++ b/src/Libraries/data_convert
--- a/src/Libraries/data_convert.zip
+++ b/src/Libraries/data_convert.zip
--- 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()) {
@@ -344,7 +343,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/xmlrpcpp
+++ b/src/Libraries/xmlrpcpp
--- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
+++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
@@ -14,7 +14,6 @@
 #include <robot_nav_msgs/Odometry.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_map_msgs/OccupancyGridUpdate.h>
-#include <robot_sensor_msgs/DepthCameraData.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
@@ -175,8 +174,7 @@ namespace robot
 		{
 		public:
 			using Ptr = std::shared_ptr<BaseNavigation>;
-			robot_nav_msgs::OccupancyGrid map_save_;
-    		std::string map_name_save_;
+
 			virtual ~BaseNavigation() {}

 			/**
@@ -185,19 +183,6 @@ namespace robot
 			 */
 			virtual void initialize(TFListenerPtr tf) = 0;

-			/**
-			 * @brief Stop internal activity (control loop, planner...) before destruction.
-			 *
-			 * The host MUST be able to quiesce the navigation core at a chosen point in its
-			 * own shutdown sequence, while the process is still fully alive — destroying the
-			 * object from a static-destruction context after main() has returned is exactly
-			 * the race that produced boost::lock_error crashes. Default is a no-op so legacy
-			 * implementations remain source-compatible; after this call the object must stop
-			 * producing velocity commands, and queries (getFeedback, getTwist...) must remain
-			 * safe to call. Safe to call multiple times.
-			 */
-			virtual void shutdown() {}
-
 			/**
 			 * @brief Set the robot's footprint (outline shape) in the global frame.
 			 *        This can be used for planning or collision checking.
@@ -259,14 +244,6 @@ namespace robot
 			 */
 			virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) = 0;

-			/**
-			 * @brief Add one coherent depth-camera sample to the navigation system.
-			 * @param topic The logical topic of the combined depth-camera stream.
-			 * @param data Depth image and matching camera intrinsics.
-			 */
-			virtual void addDepthCameraData(const std::string &topic,
-			                                robot_sensor_msgs::DepthCameraData::ConstPtr data) = 0;
-
 			/**
 			 * @brief Get a static map from the navigation system.
 			 * @param map_name The name of the map.
--- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h
+++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h
@@ -36,7 +36,6 @@

 #include <robot_nav_core2/common.h>
 #include <robot_nav_core2/costmap.h>
-#include <robot_nav_core2/exceptions.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>
--- 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
@@ -36,7 +36,6 @@

 #include <robot_nav_core2/common.h>
 #include <robot_nav_core2/costmap.h>
-#include <robot_nav_core2/exceptions.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
--- a/src/Navigations/Packages/move_base/CMakeLists.txt
+++ b/src/Navigations/Packages/move_base/CMakeLists.txt
@@ -46,7 +46,6 @@ if (NOT BUILDING_WITH_CATKIN)
    robot_cpp
    robot_move_base_msgs
    laser_filter
-    mission_adapters
  )
  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
@@ -66,13 +65,12 @@ else()
    robot_nav_2d_utils
    robot_cpp
    robot_move_base_msgs
-    mission_adapters
  )
  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS mission_adapters geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp robot_move_base_msgs
+    CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp robot_move_base_msgs
    DEPENDS Boost yaml-cpp
  )

--- a/src/Navigations/Packages/move_base/include/move_base/move_base.h
+++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h
@@ -18,7 +18,6 @@
 #include <robot_nav_core/base_global_planner.h>
 #include <robot_nav_core/base_local_planner.h>
 #include <robot_nav_core/recovery_behavior.h>
-#include <mission_adapters/mission_adapters.h>

 // boost headers
 #include <boost/thread.hpp>
@@ -126,7 +125,6 @@ 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.
@@ -141,14 +139,6 @@ namespace move_base
 			* @param point_cloud2 The point cloud2 to add.
 			*/
 		 virtual void addPointCloud2(const std::string &point_cloud2_name, robot_sensor_msgs::PointCloud2 point_cloud2) override;
-
-		 /**
-			* @brief Add one coherent depth-camera sample to the navigation system.
-			* @param topic The logical topic of the combined depth-camera stream.
-			* @param data Depth image and matching camera intrinsics.
-			*/
-		 virtual void addDepthCameraData(const std::string &topic,
-		                                 robot_sensor_msgs::DepthCameraData::ConstPtr data) override;
 
 		 /**
 			* @brief Get a static map from the navigation system.
@@ -714,4 +704,4 @@ namespace move_base

 } // namespace move_base

-#endif // NAV_MOVE_BASE_ACTION_H_
+#endif // NAV_MOVE_BASE_ACTION_H_
--- a/src/Navigations/Packages/move_base/package.xml
+++ b/src/Navigations/Packages/move_base/package.xml
@@ -49,9 +49,6 @@
  <build_depend>robot_cpp</build_depend>
  <run_depend>robot_cpp</run_depend>

-  <build_depend>mission_adapters</build_depend>
-  <run_depend>mission_adapters</run_depend>
-
  <build_depend>robot_move_base_msgs</build_depend>
  <run_depend>robot_move_base_msgs</run_depend>
  
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -266,26 +266,6 @@ 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
    {
@@ -314,6 +294,22 @@ 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
    {
@@ -491,7 +487,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);
 }
@@ -613,20 +609,6 @@ void move_base::MoveBase::addPointCloud2(const std::string &point_cloud2_name, r
  updateGlobalCostmap<robot_sensor_msgs::PointCloud2>(point_cloud2, robot_costmap_2d::LayerType::VOXEL_LAYER, point_cloud2_name);
 }

-void move_base::MoveBase::addDepthCameraData(const std::string &topic,
-                                             robot_sensor_msgs::DepthCameraData::ConstPtr data)
-{
-  if (!data)
-    return;
-
-  // robot::log_error("DEBUG");
-
-  updateLocalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
-      data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
-  updateGlobalCostmap<robot_sensor_msgs::DepthCameraData::ConstPtr>(
-      data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic);
-}
-
 robot_sensor_msgs::PointCloud2 move_base::MoveBase::getPointCloud2(const std::string &point_cloud2_name)
 {
  auto it = point_cloud2s_.find(point_cloud2_name);
@@ -747,15 +729,8 @@ 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);
      }
@@ -1449,6 +1424,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)
@@ -2889,7 +2865,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
    {
      if (tc_->isGoalReached())
      {
-        robot::log_debug("[MoveBase] Goal reached!");
+        robot::log_debug("Goal reached!");
        resetState();
        // swapPlanner(default_config_.base_global_planner);
        // disable the planner thread
@@ -3168,6 +3144,15 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
  }

  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
+  // robot_geometry_msgs::PoseStamped goal_pose, global_pose;
+  // goal_pose = goal_pose_msg;
+
+  // goal_pose.header.stamp = robot::Time(); // latest available
+  // try
+  // {
+  //   tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+  //   tf3::doTransform(goal_pose, global_pose, transform);
+  // }
  robot_geometry_msgs::PoseStamped global_pose;
  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
  robot_geometry_msgs::PoseStamped goal_pose;
@@ -3233,16 +3218,17 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
  {
    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)
+    if (!path.poses.empty())
    {
-      robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
-      pose.header.stamp = robot::Time::now();
-      global_path.poses.push_back(goalToGlobalFrame(pose));
+      path.header.stamp = path.poses[0].header.stamp;
+      path.header.frame_id = path.poses[0].header.frame_id;
    }
-    global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
+    else
+    {
+      path.header.stamp = robot::Time::now();
+      path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
+    }
+    global_data_.plan = robot_nav_2d_utils::pathToPath(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);
@@ -3263,4 +3249,4 @@ robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
  return std::make_shared<move_base::MoveBase>();
 }

-BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase)
+BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase)
Author	SHA1	Message	Date
hoangson02	587e2deb60	merge branch 3.0	2026-03-23 03:03:08 +00:00
hoangson02	e6dd3e97b5	Merge remote-tracking branch 'origin/3.0' into awc-devel	2026-03-22 10:27:54 +00:00
hoangson02	768a257b33	update find path file plugin .so	2026-03-22 05:00:23 +00:00
hoangson02	3c8e1cdaf5	update	2026-03-22 03:00:05 +00:00
hoangson02	cf0c6e7caf	Merge remote-tracking branch 'origin/3.0' into awc-devel	2026-03-22 02:58:56 +00:00
hoangson02	6ff54e4154	Merge remote-tracking branch 'origin/3.0' into awc-devel	2026-03-21 12:30:57 +00:00
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