add new file nav_c_api2 and function

.gitignore

@@ -421,3 +421,4 @@ FodyWeavers.xsd
 build
 install
 devel
+

.gitmodules

@@ -28,3 +28,6 @@
 [submodule "src/Libraries/xmlrpcpp"]
 	path = src/Libraries/xmlrpcpp
 	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
+[submodule "src/Libraries/laser_filter"]
+	path = src/Libraries/laser_filter
+	url = https://git.pnkr.asia/DuongTD/laser_filter.git

CATKIN_BUILD_README.md

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

CMAKE_BUILD_README.md

@@ -51,6 +51,7 @@ sudo apt-get install -y \
     libyaml-cpp-dev \
     libpcl-dev \
     libgoogle-glog-dev
+sudo apt install liborocos-kdl-dev
 
 # Optional: Cài đặt Google Test (nếu muốn build tests)
 sudo apt-get install -y libgtest-dev

CMakeLists.txt

@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
@@ -146,6 +150,83 @@ 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)
@@ -160,5 +241,3 @@ endif()
 message(STATUS "========================================")
 message(STATUS "All packages configured successfully")
 message(STATUS "========================================")
-
-

config/config/obstacle_layer_params.yaml

@@ -1,4 +1,5 @@
 obstacle_layer:
+  enabled: true
   track_unknown_space: true
   transform_tolerance: 0.2
   topic: "map"

config/config/voxel_layer_params.yaml

@@ -8,4 +8,8 @@ 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

config/costmap_common_params.yaml

@@ -1,12 +1,7 @@
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-base_local_planner: LocalPlannerAdapter
-base_global_planner: CustomPlanner
-
-robot_base_frame: base_footprint
+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
@@ -18,7 +13,7 @@ navigation_map:
   map_file: maze
 
 virtual_walls_map:
-  map_topic: /virtual_walls/map
+  map_topic: /map
   namespace: /virtual_walls
   map_pkg: managerments
   map_file: maze
@@ -26,38 +21,80 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
-    topic: /f_scan
-    data_type: LaserScan
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    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: false
-    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: false
-    min_obstacle_height: 0.0
-    max_obstacle_height: 0.25
-  b_scan_clearing:
+  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:
     topic: /b_scan
     data_type: LaserScan
     clearing: true
-    marking: false
-    inf_is_valid: false
+    marking: true
+    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

config/costmap_global_params.yaml

@@ -1,10 +1,10 @@
 global_costmap:
   library_path: libplugins
-  robot_base_frame: base_footprint
+  robot_base_frame: base_link
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 2.0
+  raytrace_range: 3.0
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_global_params_plugins_no_virtual_walls.yaml

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

config/costmap_local_params.yaml

@@ -1,11 +1,11 @@
 local_costmap: 
   library_path: libplugins
   global_frame: odom
-  robot_base_frame: base_footprint
+  robot_base_frame: base_link
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 2.0
+  raytrace_range: 3.0
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/costmap_local_params_plugins_no_virtual_walls.yaml

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

config/dock_global_params.yaml

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

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,55 @@
+HybridLocalPlanner:
+# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
+# HybridLocalPlanner:
+    library_path: libhybrid_local_planner
+    # 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
+    acc_lim_theta: 2.0
+    min_turn_radius: 0.0
+    robot_radius: 0.1
+    footprint_padding: 0.08
+    use_footprint: true
+
+    # GoalTolerance
+    xy_goal_tolerance: 0.02
+    yaw_goal_tolerance: 0.02
+
+    # Trajectory
+    max_global_plan_lookahead_dist: 3.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_goal_sep: 0.05
+    predict_time: 3.0
+    sim_period: 0.1
+    sim_time_samples: 10
+
+    # 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
+
+    #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]

config/maker_sources.yaml

@@ -9,14 +9,14 @@ trolley:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được   
     timeout: 60.0
-    vel_x: 0.25
+    vel_x: 0.2
     vel_theta: 0.3
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -30,7 +30,7 @@ trolley:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 charger:
   plugins:
@@ -41,13 +41,13 @@ charger:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60
-    vel_x: 0.1
+    vel_x: 0.15
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 dock_station:
   plugins:
@@ -102,7 +102,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
     
   qrcode:
     maker_goal_frame: qr_trolley
@@ -116,7 +116,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 undock_station_2:
   plugins:
@@ -135,7 +135,7 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -149,4 +149,4 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4

config/mission_adapters_params.yaml

@@ -0,0 +1,42 @@
+# 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

config/move_base_common_params.yaml

@@ -1,14 +1,46 @@
+position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PriestLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner #CustomPlanner SBPLLatticePlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
-controller_frequency: 30.0   # run controller at 15.0 Hz
+controller_frequency: 30.0   # run controller at 30.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_behavior_enabled: true
+## 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: [       
                       {name: aggressive_reset, type: ClearCostmapRecovery},
                       {name: conservative_reset, type: ClearCostmapRecovery},                      

config/mppi_local_planner_params.yaml

@@ -0,0 +1,98 @@
+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

config/mprim/ekf.yaml

@@ -62,7 +62,7 @@ publish_acceleration: false
 #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
 map_frame: map                                   # Defaults to "map" if unspecified
 odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
-base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+base_link_frame: $(arg tf_prefix)base_link  # Defaults to "base_link" if unspecified
 world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
 
 # The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,

config/mprim/localization.yaml

@@ -25,7 +25,7 @@ Amcl:
   update_min_d: 0.05
   update_min_a: 0.05
   odom_frame_id: odom
-  base_frame_id: base_footprint
+  base_frame_id: base_link
   global_frame_id: map
   resample_interval: 1
   transform_tolerance: 0.2

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

config/mprim/ros_diff_drive_controller.yaml

@@ -25,7 +25,7 @@ wheel_radius_multiplier    : 1.0 # default: 1.0
 cmd_vel_timeout: 1.0
 
 # frame_ids (same as real MiR platform)
-base_frame_id: base_footprint # default: base_link base_footprint
+base_frame_id: base_link # default: base_link base_link
 odom_frame_id: odom           # default: odom 
 
 # Velocity and acceleration limits

config/pnkx_local_planner_params.yaml

@@ -1,5 +1,5 @@
-yaw_goal_tolerance: 0.017  
-xy_goal_tolerance: 0.02 
+yaw_goal_tolerance: 0.02  
+xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.05 
 
 LocalPlannerAdapter:
@@ -50,15 +50,15 @@ LimitedAccelGenerator:
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
   min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.5
+  acc_lim_x: 3.0
   acc_lim_y: 0.0      # diff drive robot
   acc_lim_theta: 1.5
-  decel_lim_x: -1.5
+  decel_lim_x: -3.0
   decel_lim_y: -0.0
-  decel_lim_theta: -1.5
+  decel_lim_theta: -2.0
 
   # Whether to split the path into segments or not
   split_path: true
@@ -74,11 +74,19 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
-  angle_threshold: 0.47
+  xy_local_goal_tolerance: 0.05
+  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 
+  # Kalman filter tuning (filters v and w commands)
+  kf_q_v: 0.25
+  kf_q_w: 0.8
+  kf_r_v: 0.05
+  kf_r_w: 0.08
+  kf_p0: 0.5
+  kf_filter_angular: false
+
   # Lookahead
   use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
   # only when false:
@@ -98,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -111,7 +119,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
@@ -135,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -148,7 +156,7 @@ MKTAlgorithmDiffGoStraight:
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  xy_local_goal_tolerance: 0.02
+  xy_local_goal_tolerance: 0.05
   angle_threshold: 0.47
   index_samples: 60
   follow_step_path: true
@@ -172,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1

config/priest_local_planner_params.yaml

@@ -0,0 +1,56 @@
+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

config/recovery_behaviors_params.yaml

@@ -0,0 +1,170 @@
+# 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

config/sbpl_global_params.yaml

@@ -0,0 +1,15 @@
+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

config/stanley_local_planner_params.yaml

@@ -0,0 +1,115 @@
+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

examples/NavigationExample/NavigationAPI.cs

@@ -0,0 +1,299 @@
+using System;
+using System.Runtime.InteropServices;
+using NavigationExample;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# P/Invoke wrapper for Navigation C API
+    /// </summary>
+    public class NavigationAPI
+    {
+        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
+        // For Windows: "nav_c_api.dll"
+        // For macOS: "libnav_c_api.dylib"
+
+        // ============================================================================
+        // Enums
+        // ============================================================================
+        public enum NavigationState
+        {
+            Pending = 0,
+            Active = 1,
+            Preempted = 2,
+            Succeeded = 3,
+            Aborted = 4,
+            Rejected = 5,
+            Preempting = 6,
+            Recalling = 7,
+            Recalled = 8,
+            Lost = 9,
+            Planning = 10,
+            Controlling = 11,
+            Clearing = 12,
+            Paused = 13
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavFeedback
+        {
+            public NavigationState navigation_state;
+            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
+            public Pose2D current_pose;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool goal_checked;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_ready;
+        }
+
+
+        /// <summary>Planner data output (plan, costmap, footprint).</summary>
+        [StructLayout(LayoutKind.Sequential)]
+        public struct PlannerDataOutput
+        {
+            public Path2D plan;
+            public OccupancyGrid costmap;
+            public OccupancyGridUpdate costmap_update;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_costmap_updated;
+            public PolygonStamped footprint;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavigationHandle
+        {
+            public IntPtr ptr;
+        }
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_create(string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_set_data(
+            uint seq,
+            uint sec,
+            uint nsec,
+            string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern Time time_create();
+
+        /// <summary>Free a string allocated by the API (strdup).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void nav_c_api_free_string(IntPtr str);
+
+        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern IntPtr navigation_state_to_string(NavigationState state);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
+
+
+        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
+        public static string MarshalString(IntPtr p)
+        {
+            if (p == IntPtr.Zero) return string.Empty;
+            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
+        }
+
+        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
+        public static void navigation_free_feedback(ref NavFeedback feedback)
+        {
+            if (feedback.feed_back_str != IntPtr.Zero)
+            {
+                nav_c_api_free_string(feedback.feed_back_str);
+                feedback.feed_back_str = IntPtr.Zero;
+            }
+        }
+
+        // ============================================================================
+        // Navigation Handle Management
+        // ============================================================================
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern NavigationHandle navigation_create();
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_destroy(NavigationHandle handle);
+
+        /// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
+
+        /// <summary>Send a goal for the robot to navigate to (global frame).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);
+
+        /// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
+
+        /// <summary>Rotate in place to align with target orientation (radians).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_pause(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_resume(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_cancel(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
+
+        // ============================================================================
+        // Navigation Data Management
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
+    
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_data(NavigationHandle handle);
+
+        /// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+    }
+}

examples/NavigationExample/NavigationExample.csproj

@@ -1,12 +1,11 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
-    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <TargetFramework>net10.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>
-    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.5" />
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.7" />
   </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">

examples/NavigationExample/Program.cs

@@ -6,6 +6,8 @@ using System.Runtime.CompilerServices;
 using System.Text.RegularExpressions;
 using SixLabors.ImageSharp;
 using SixLabors.ImageSharp.PixelFormats;
+using NavigationExample;
+
 
 namespace NavigationExample
 {
@@ -19,366 +21,6 @@ namespace NavigationExample
         public double OccupiedThresh;
         public double FreeThresh;
     }
-    /// <summary>
-    /// C# P/Invoke wrapper for Navigation C API
-    /// </summary>
-    public class NavigationAPI
-    {
-        private const string DllName = "libnav_c_api.so"; // Linux
-        // For Windows: "nav_c_api.dll"
-        // For macOS: "libnav_c_api.dylib"
-
-        // ============================================================================
-        // Enums
-        // ============================================================================
-        public enum NavigationState
-        {
-            Pending = 0,
-            Active = 1,
-            Preempted = 2,
-            Succeeded = 3,
-            Aborted = 4,
-            Rejected = 5,
-            Preempting = 6,
-            Recalling = 7,
-            Recalled = 8,
-            Lost = 9,
-            Planning = 10,
-            Controlling = 11,
-            Clearing = 12,
-            Paused = 13
-        }
-
-        // ============================================================================
-        // Structures
-        // ============================================================================
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Point
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Quaternion
-        {
-            public double x;
-            public double y;
-            public double z;
-            public double w;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Position
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose
-        {
-            public Position position;
-            public Quaternion orientation;
-        }
-
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist
-        {
-            public Vector3 linear;
-            public Vector3 angular;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Header
-        {
-            public uint seq;
-            public uint sec;
-            public uint nsec;
-            public IntPtr frame_id; // char*
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseStamped
-        {
-            public Header header;
-            public Pose pose;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist2DStamped
-        {
-            public Header header;
-            public Twist2D velocity;
-        }
-
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavigationHandle
-        {
-            public IntPtr ptr;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct TFListenerHandle
-        {
-            public IntPtr ptr;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct LaserScan
-        {
-            public Header header;
-            public float angle_min;
-            public float angle_max;
-            public float angle_increment;
-            public float time_increment;
-            public float scan_time;
-            public float range_min;
-            public float range_max;
-            public IntPtr ranges;
-            public UIntPtr ranges_count;
-            public IntPtr intensities;
-            public UIntPtr intensities_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseWithCovariance
-        {
-            public Pose pose;
-            public IntPtr covariance;
-            public UIntPtr covariance_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct TwistWithCovariance    {
-            public Twist twist;
-            public IntPtr covariance;
-            public UIntPtr covariance_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Odometry
-        {
-            public Header header;
-            public IntPtr child_frame_id;
-            public PoseWithCovariance pose;
-            public TwistWithCovariance twist;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct OccupancyGrid
-        {
-            public Header header;
-            public MapMetaData info;
-            public IntPtr data;
-            public UIntPtr data_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct MapMetaData
-        {
-            public Time map_load_time;
-            public float resolution;
-            public uint width;
-            public uint height;
-            public Pose origin;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Time
-        {
-            public uint sec;
-            public uint nsec;
-        }
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern Header header_create(string frame_id);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern Header header_set_data(
-            uint seq,
-            uint sec,
-            uint nsec,
-            string frame_id);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern Time time_create();
-
-        /// <summary>Free a string allocated by the API (strdup).</summary>
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
-
-
-        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
-        public static string MarshalString(IntPtr p)
-        {
-            if (p == IntPtr.Zero) return string.Empty;
-            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
-        }
-
-        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
-        public static void navigation_free_feedback(ref NavFeedback feedback)
-        {
-            if (feedback.feed_back_str != IntPtr.Zero)
-            {
-                nav_c_api_free_string(feedback.feed_back_str);
-                feedback.feed_back_str = IntPtr.Zero;
-            }
-        }
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern TFListenerHandle tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(TFListenerHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool tf_listener_set_static_transform(
-            TFListenerHandle tf_handle,
-            string parent_frame,
-            string child_frame,
-            double x, double y, double z,
-            double qx, double qy, double qz, double qw);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern NavigationHandle navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_pause(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_resume(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_cancel(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
-        // ============================================================================
-        // Navigation Data Management
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
-    
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
-    
-    }
 
     // ============================================================================
     // Example Usage
@@ -497,86 +139,81 @@ namespace NavigationExample
 
         static void Main(string[] args)
         {
-            // Create TF listener
-            NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
-            if (tfHandle.ptr == IntPtr.Zero)
+            // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
+            IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
+            if (tf3Buffer == IntPtr.Zero)
             {
-                LogError("Failed to create TF listener");
+                LogError("Failed to create tf3 buffer (libtf3.so may be missing)");
+                return;
+            }
+            Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}");
+            string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?";
+            Console.WriteLine($"[TF3] {version}");
+
+            // Inject static transforms: map -> odom -> base_footprint -> base_link
+            var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1);
+            var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1);
+            var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1);
+            if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true))
+            {
+                LogError("Failed to set static TF");
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
-            // If you already publish TF from localization/odometry, you can remove this call.
-            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
-                                                               0, 0, 0,
-                                                               0, 0, 0, 1))
-            {
-                LogError("Failed to inject static TF map -> odom");
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint",
-                                                               0, 0, 0,
-                                                               0, 0, 0, 1))
-            {
-                LogError("Failed to inject static TF map -> base_link");
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link",
-                                                               0, 0, 0,
-                                                               0, 0, 0, 1))
-            {
-                LogError("Failed to inject static TF map -> base_link");
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            // Create navigation instance
+            // Create navigation instance and initialize with tf3 buffer
             NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
             if (navHandle.ptr == IntPtr.Zero)
             {
                 LogError("Failed to create navigation instance");
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Initialize navigation
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
+            if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
             {
-                LogError("Failed to initialize navigation");
+                LogError("Failed to initialize navigation with tf3 buffer");
                 NavigationAPI.navigation_destroy(navHandle);
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
+            while (true)
+            {
+                NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
+                if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
+                {
+                    if (feedback.is_ready)
+                    {
+                        Console.WriteLine("Navigation is ready");
+                        break;
+                    }
+                    else
+                    {
+                        Console.WriteLine("Navigation is not ready");
+                    }
+                }
+                System.Threading.Thread.Sleep(100);
+            }
+            Console.WriteLine("[NavigationExample] Navigation initialized successfully");
+
+
             // Set robot footprint
-            NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
+            Point[] footprint = new Point[]
             {
-                new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
-                new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.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 },
+                new Point { x = -0.3, y = -0.2, z = 0.0 }
             };
             NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
 
-            // Get navigation feedback
-            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
-            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
-            {
-                IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state);
-                string stateStr = NavigationAPI.MarshalString(stateStrPtr);
-                NavigationAPI.nav_c_api_free_string(stateStrPtr);
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
-                NavigationAPI.navigation_free_feedback(ref feedback);
-            }
   
             IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
-            NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
-            NavigationAPI.LaserScan fscanHandle;
+            Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
+            LaserScan fscanHandle;
             fscanHandle.header = fscanHeader;
             fscanHandle.angle_min = -1.57f;
             fscanHandle.angle_max = 1.57f;
@@ -594,8 +231,8 @@ namespace NavigationExample
             NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);
 
             IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
-            NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
-            NavigationAPI.LaserScan bscanHandle;
+            Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
+            LaserScan bscanHandle;
             bscanHandle.header = bscanHeader;
             bscanHandle.angle_min = 1.57f;
             bscanHandle.angle_max = -1.57f;
@@ -613,8 +250,8 @@ namespace NavigationExample
             NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan",  bscanHandle);
 
             IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
-            NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
-            NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry();
+            Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
+            Odometry odometryHandle = new Odometry();
             odometryHandle.header = odometryHeader;
             IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
             odometryHandle.child_frame_id = childFrameId;
@@ -650,8 +287,7 @@ namespace NavigationExample
             NavigationAPI.navigation_add_odometry(navHandle, "odometry",  odometryHandle);
 
             // Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
-            string mapYamlPath = args.Length > 0 ? args[0] : Path.Combine(
-                "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws/src/Managerments/maps/maze", "maze.yaml");
+            string mapYamlPath =  "maze.yaml";
             int mapWidth, mapHeight;
             byte[] data;
             MazeMapConfig mapConfig;
@@ -686,13 +322,13 @@ namespace NavigationExample
                 Console.WriteLine("maze.yaml not found, using default map 3x10");
             }
 
-            NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
-            NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
+            Time mapLoadTime = NavigationAPI.time_create();
+            MapMetaData mapMetaData = new MapMetaData();
             mapMetaData.map_load_time = mapLoadTime;
             mapMetaData.resolution = mapConfig.Resolution;
             mapMetaData.width = (uint)mapWidth;
             mapMetaData.height = (uint)mapHeight;
-            mapMetaData.origin = new NavigationAPI.Pose();
+            mapMetaData.origin = new Pose();
             mapMetaData.origin.position.x = mapConfig.OriginX;
             mapMetaData.origin.position.y = mapConfig.OriginY;
             mapMetaData.origin.position.z = mapConfig.OriginZ;
@@ -700,7 +336,7 @@ namespace NavigationExample
             mapMetaData.origin.orientation.y = 0.0;
             mapMetaData.origin.orientation.z = 0.0;
             mapMetaData.origin.orientation.w = 1.0;
-            NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid();
+            OccupancyGrid occupancyGrid = new OccupancyGrid();
             IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
             occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
             occupancyGrid.info = mapMetaData;
@@ -709,29 +345,153 @@ namespace NavigationExample
             occupancyGrid.data_count = new UIntPtr((uint)data.Length);
             Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count);
             Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}",
-                Marshal.SizeOf<NavigationAPI.OccupancyGrid>(),
-                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data"),
-                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data_count"));
+                Marshal.SizeOf<OccupancyGrid>(),
+                Marshal.OffsetOf<OccupancyGrid>("data"),
+                Marshal.OffsetOf<OccupancyGrid>("data_count"));
     
             NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
 
-            System.Threading.Thread.Sleep(500);
-
-            NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
+            Twist2DStamped twist = new Twist2DStamped();
             if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
             {
                 Console.WriteLine(
                     "Twist: {0}, {1}, {2}, {3}", 
                     NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
-            }            
-            // Cleanup
-            NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
+            }  
+
+            // Build order (thao cách bom order): header + nodes + edges giống C++
+            Order order = new Order();
+            order.headerId = 1;
+            order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00");
+            order.version = Marshal.StringToHGlobalAnsi("1.0.0");
+            order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer");
+            order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number");
+            order.orderId = Marshal.StringToHGlobalAnsi("Order ID");
+            order.orderUpdateId = 1;
+
+            // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); }
+            int nodeCount = 1;
+            order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf<Node>() * nodeCount);
+            order.nodes_count = new UIntPtr((uint)nodeCount);
+            Node node1 = new Node();
+            node1.nodeId = Marshal.StringToHGlobalAnsi("node-1");
+            node1.sequenceId = 0;
+            node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node");
+            node1.released = 0;
+            node1.nodePosition.x = 1.0;
+            node1.nodePosition.y = 1.0;
+            node1.nodePosition.theta = 0.0;
+            node1.nodePosition.allowedDeviationXY = 0.1f;
+            node1.nodePosition.allowedDeviationTheta = 0.05f;
+            node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map");
+            node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi("");
+            node1.actions = IntPtr.Zero;
+            node1.actions_count = UIntPtr.Zero;
+            Marshal.StructureToPtr(node1, order.nodes, false);
+
+            // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...)
+            order.edges = IntPtr.Zero;
+            order.edges_count = UIntPtr.Zero;
+            order.zoneSetId = Marshal.StringToHGlobalAnsi("");
+
+            PoseStamped goal = new PoseStamped();
+            goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
+            goal.pose.position.x = 0.01;
+            goal.pose.position.y = 0.01;
+            goal.pose.position.z = 0.0;
+            goal.pose.orientation.x = 0.0;
+            goal.pose.orientation.y = 0.0;
+            goal.pose.orientation.z = 0.0;
+            goal.pose.orientation.w = 1.0;
+
+
+            // Console.WriteLine("Docking to docking_point");
+            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);
+
+            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
+
+            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
+            NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
+            NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
+
+            // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
+
+            while (true) 
+            {
+                System.Threading.Thread.Sleep(100);
+            //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
+            //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
+            //     {
+            //         if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded)
+            //         {
+            //             Console.WriteLine("Navigation is Succeeded");
+            //             break;
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
+            //     if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
+            //     {
+            //         int n = (int)(uint)globalData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(globalData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(globalData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Global Costmap is not updated");
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput();
+            //     if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) 
+            //     {
+            //         int n = (int)(uint)localData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(localData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(localData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Local Costmap is not updated");
+            //         }
+            //     }
+            }
+            // Cleanup (destroy nav first, then tf3 buffer)
 
             NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
+            TF3API.tf3_buffer_destroy(tf3Buffer);
             Console.WriteLine("Press any key to exit...");
-            Console.ReadKey();
+            try
+            {
+                Console.ReadKey(intercept: true);
+            }
+            catch (InvalidOperationException)
+            {
+                // Running without a real console (e.g. redirected/automated run).
+            }
         }
     }
 }
-

examples/NavigationExample/TF3API.cs

@@ -0,0 +1,133 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+
+   // ============================================================================
+    // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
+    // ============================================================================
+    public static class TF3API
+    {
+        private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib
+
+        public enum TF3_ErrorCode
+        {
+            TF3_OK = 0,
+            TF3_ERROR_LOOKUP = 1,
+            TF3_ERROR_CONNECTIVITY = 2,
+            TF3_ERROR_EXTRAPOLATION = 3,
+            TF3_ERROR_INVALID_ARGUMENT = 4,
+            TF3_ERROR_TIMEOUT = 5,
+            TF3_ERROR_UNKNOWN = 99
+        }
+
+        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
+        public struct TF3_Transform
+        {
+            public long timestamp_sec;
+            public long timestamp_nsec;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string frame_id;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string child_frame_id;
+            public double translation_x;
+            public double translation_y;
+            public double translation_z;
+            public double rotation_x;
+            public double rotation_y;
+            public double rotation_z;
+            public double rotation_w;
+        }
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_buffer_create(int cache_time_sec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_buffer_destroy(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_set_transform(
+            IntPtr buffer,
+            ref TF3_Transform transform,
+            string authority,
+            [MarshalAs(UnmanagedType.I1)] bool is_static);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform_full(
+            IntPtr buffer,
+            string target_frame,
+            long target_time_sec,
+            long target_time_nsec,
+            string source_frame,
+            long source_time_sec,
+            long source_time_nsec,
+            string fixed_frame,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_can_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            System.Text.StringBuilder error_msg,
+            int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_clear(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_get_current_time(out long sec, out long nsec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_get_version();
+
+        /// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
+        public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
+            double tx = 0, double ty = 0, double tz = 0,
+            double qx = 0, double qy = 0, double qz = 0, double qw = 1)
+        {
+            var t = new TF3_Transform();
+            t.timestamp_sec = 0;
+            t.timestamp_nsec = 0;
+            t.frame_id = parentFrame ?? "";
+            t.child_frame_id = childFrame ?? "";
+            t.translation_x = tx;
+            t.translation_y = ty;
+            t.translation_z = tz;
+            t.rotation_x = qx;
+            t.rotation_y = qy;
+            t.rotation_z = qz;
+            t.rotation_w = qw;
+            return t;
+        }
+    }
+}

examples/NavigationExample/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

examples/NavigationExample/msgs/CommonMsgs.cs

@@ -0,0 +1,274 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    // ============================================================================
+    // Structures
+    // ============================================================================
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Quaternion
+    {
+        public double x;
+        public double y;
+        public double z;
+        public double w;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Position
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose
+    {
+        public Point position;
+        public Quaternion orientation;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Vector3
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist
+    {
+        public Vector3 linear;
+        public Vector3 angular;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Header
+    {
+        public uint seq;
+        public uint sec;
+        public uint nsec;
+        public IntPtr frame_id; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseStamped
+    {
+        public Header header;
+        public Pose pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2DStamped
+    {
+        public Header header;
+        public Twist2D velocity;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct LaserScan
+    {
+        public Header header;
+        public float angle_min;
+        public float angle_max;
+        public float angle_increment;
+        public float time_increment;
+        public float scan_time;
+        public float range_min;
+        public float range_max;
+        public IntPtr ranges;
+        public UIntPtr ranges_count;
+        public IntPtr intensities;
+        public UIntPtr intensities_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud
+    {
+        public Header header;
+        public IntPtr points;
+        public UIntPtr points_count;
+        public IntPtr channels;
+        public UIntPtr channels_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud2
+    {
+        public Header header;
+        public uint height;
+        public uint width;
+        public IntPtr fields;
+        public UIntPtr fields_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_bigendian;
+        public uint point_step;
+        public uint row_step;
+        public IntPtr data;
+        public UIntPtr data_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_dense;
+    } 
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseWithCovariance
+    {
+        public Pose pose;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct TwistWithCovariance    {
+        public Twist twist;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Odometry
+    {
+        public Header header;
+        public IntPtr child_frame_id;
+        public PoseWithCovariance pose;
+        public TwistWithCovariance twist;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGrid
+    {
+        public Header header;
+        public MapMetaData info;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct MapMetaData
+    {
+        public Time map_load_time;
+        public float resolution;
+        public uint width;
+        public uint height;
+        public Pose origin;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Time
+    {
+        public uint sec;
+        public uint nsec;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point32
+    {
+        public float x;
+        public float y;
+        public float z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2DStamped
+    {
+        public Header header;
+        public Pose2D pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Polygon
+    {
+        public IntPtr points;
+        public UIntPtr points_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PolygonStamped
+    {
+        public Header header;
+        public Polygon polygon;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGridUpdate
+    {
+        public Header header;
+        public int x;
+        public int y;
+        public uint width;
+        public uint height;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Path2D
+    {
+        public Header header;
+        public IntPtr poses;
+        public UIntPtr poses_count;
+    }
+
+    /// <summary>Name (char*) + OccupancyGrid for static map list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedOccupancyGrid
+    {
+        public IntPtr name;
+        public OccupancyGrid grid;
+    }
+
+    /// <summary>Name (char*) + LaserScan for laser scan list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedLaserScan
+    {
+        public IntPtr name;
+        public LaserScan scan;
+    }
+
+    /// <summary>Name (char*) + PointCloud for point cloud list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud
+    {
+        public IntPtr name;
+        public PointCloud cloud;
+    }
+
+    /// <summary>Name (char*) + PointCloud2 for point cloud2 list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud2
+    {
+        public IntPtr name;
+        public PointCloud2 cloud;
+    }
+
+
+}

examples/NavigationExample/msgs/ProtocolMsgsTypes.cs

@@ -0,0 +1,109 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# struct layout cho protocol_msgs (robot_protocol_msgs C API).
+    /// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ControlPoint
+    {
+        public double x;
+        public double y;
+        public double weight;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ActionParameter
+    {
+        public IntPtr key;   // char*
+        public IntPtr value; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NodePosition
+    {
+        public double x;
+        public double y;
+        public double theta;
+        public float allowedDeviationXY;
+        public float allowedDeviationTheta;
+        public IntPtr mapId;          // char*
+        public IntPtr mapDescription; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Trajectory
+    {
+        public uint degree;
+        public IntPtr knotVector;           // double*
+        public UIntPtr knotVector_count;
+        public IntPtr controlPoints;        // ControlPoint*
+        public UIntPtr controlPoints_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Action
+    {
+        public IntPtr actionType;       // char*
+        public IntPtr actionId;         // char*
+        public IntPtr actionDescription;// char*
+        public IntPtr blockingType;     // char*
+        public IntPtr actionParameters; // ActionParameter*
+        public UIntPtr actionParameters_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Node
+    {
+        public IntPtr nodeId;           // char*
+        public int sequenceId;
+        public IntPtr nodeDescription;  // char*
+        public byte released;
+        public NodePosition nodePosition;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Edge
+    {
+        public IntPtr edgeId;           // char*
+        public int sequenceId;
+        public IntPtr edgeDescription; // char*
+        public byte released;
+        public IntPtr startNodeId;     // char*
+        public IntPtr endNodeId;       // char*
+        public double maxSpeed;
+        public double maxHeight;
+        public double minHeight;
+        public double orientation;
+        public IntPtr orientationType; // char*
+        public IntPtr direction;       // char*
+        public byte rotationAllowed;
+        public double maxRotationSpeed;
+        public Trajectory trajectory;
+        public double length;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Order
+    {
+        public int headerId;
+        public IntPtr timestamp;        // char*
+        public IntPtr version;         // char*
+        public IntPtr manufacturer;    // char*
+        public IntPtr serialNumber;    // char*
+        public IntPtr orderId;         // char*
+        public uint orderUpdateId;
+        public IntPtr nodes;           // Node*
+        public UIntPtr nodes_count;
+        public IntPtr edges;           // Edge*
+        public UIntPtr edges_count;
+        public IntPtr zoneSetId;      // char*
+    }
+}

examples/run_example.sh

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

pnkx_nav_core.sln

@@ -0,0 +1,29 @@
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.5.2.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(NestedProjects) = preSolution
+		{995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305}
+	EndGlobalSection
+EndGlobal

src/APIs/c_api/CMakeLists.txt

@@ -21,6 +21,9 @@ set(PACKAGES_DIR
     robot_time
     robot_cpp
     geometry_msgs
+    angles
+    data_convert
+    robot_protocol_msgs
 )
 
 # Thư mục include

src/APIs/c_api/NAV_C_API_2_GUIDE.md

@@ -0,0 +1,859 @@
+# `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);
+}
+```

src/APIs/c_api/include/convertor.h

@@ -13,6 +13,10 @@
 #include "geometry_msgs/Pose2D.h"
 #include "nav_2d_msgs/Twist2D.h"
 #include "nav_2d_msgs/Twist2DStamped.h"
+#include "nav_2d_msgs/Path2D.h"
+#include "map_msgs/OccupancyGridUpdate.h"
+#include "geometry_msgs/PolygonStamped.h"
+#include "protocol_msgs/Order.h"
 
 // C++
 #include <robot_sensor_msgs/LaserScan.h>
@@ -24,6 +28,10 @@
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
+#include <robot_protocol_msgs/Order.h>
 
 #include <move_base_core/navigation.h>
 #include <move_base_core/common.h>
@@ -49,6 +57,21 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu
  */
 void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out);
 
+/**
+ * @brief Convert C++ Path2D to C Path2D
+ */
+void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out);
+
+/**
+ * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate
+ */
+void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out);
+
+/**
+ * @brief Convert C++ PolygonStamped to C PolygonStamped
+ */
+void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out);
+
 /**
  * @brief Convert C++ LaserScan to C LaserScan
  * @param cpp C++ LaserScan
@@ -150,4 +173,34 @@ robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped
  */
 Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped);
 
+/**
+ * @brief Convert C Order to C++ Order
+ * @param order C Order
+ * @return C++ Order
+ */
+robot_protocol_msgs::Order convert2CppOrder(const Order& order);
+
+/**
+ * @brief Convert C++ Order to C Order
+ * @param cpp_order C++ Order
+ * @return C Order (caller must call order_free when done to release memory)
+ */
+Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
+
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
+/**
+ * @brief Free dynamic memory held by an Order returned from convert2COrder
+ * @param order Order to free (pointers and counts are zeroed)
+ */
+#ifdef __cplusplus
+extern "C"
+#endif
+void order_free(Order* order);
+
 #endif // C_API_CONVERTOR_H

src/APIs/c_api/include/convertor_2.h

@@ -0,0 +1,51 @@
+#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

src/APIs/c_api/include/nav_c_api.h

@@ -24,21 +24,14 @@ extern "C"
   #include "sensor_msgs/PointCloud.h"
   #include "sensor_msgs/PointCloud2.h"
   #include "geometry_msgs/PolygonStamped.h"
+  #include "protocol_msgs/Order.h"
 
   typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid;
   typedef struct { char *name; LaserScan scan; } NamedLaserScan;
   typedef struct { char *name; PointCloud cloud; } NamedPointCloud;
   typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2;
-
-  typedef struct
-  {
-    void *ptr;
-  } NavigationHandle;
-  
-  typedef struct
-  {
-    void *ptr;
-  } TFListenerHandle;
+  typedef void* NavigationHandle;
+  typedef void* TFListenerHandle;
   
   typedef enum
   {
@@ -98,53 +91,18 @@ extern "C"
    */
   void navigation_destroy(NavigationHandle handle);
 
-  /**
-   * @brief Create a TF listener instance
-   * @return TF listener handle, or NULL on failure
-   */
-  TFListenerHandle tf_listener_create(void);
-
-  /**
-   * @brief Destroy a TF listener instance
-   * @param handle TF listener handle to destroy
-   */
-  void tf_listener_destroy(TFListenerHandle handle);
-
-  /**
-   * @brief Inject a static transform into the TF buffer.
-   *
-   * This is a convenience for standalone usage where no external TF publisher exists yet.
-   * It will create/ensure the frames exist and become transformable.
-   *
-   * @param tf_handle TF listener handle
-   * @param parent_frame Parent frame id (e.g. "map")
-   * @param child_frame Child frame id (e.g. "base_link")
-   * @param x Translation x (meters)
-   * @param y Translation y (meters)
-   * @param z Translation z (meters)
-   * @param qx Rotation quaternion x
-   * @param qy Rotation quaternion y
-   * @param qz Rotation quaternion z
-   * @param qw Rotation quaternion w
-   * @return true on success, false on failure
-   */
-  bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
-                                        const char *parent_frame,
-                                        const char *child_frame,
-                                        double x, double y, double z,
-                                        double qx, double qy, double qz, double qw);
-
   // ============================================================================
   // Navigation Interface Methods
   // ============================================================================
 
   /**
-   * @brief Initialize the navigation system
+   * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create).
+   * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy).
    * @param handle Navigation handle
-   * @param tf_handle TF listener handle
+   * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3)
    * @return true on success, false on failure
    */
-  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer);
 
   /**
    * @brief Set the robot's footprint (outline shape)
@@ -169,51 +127,63 @@ extern "C"
    * @brief Send a goal for the robot to navigate to
    * @param handle Navigation handle
    * @param goal Target pose in the global frame
-   * @param xy_goal_tolerance Acceptable error in X/Y (meters)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if goal was accepted and sent successfully
    */
-  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal,
-                          double xy_goal_tolerance, double yaw_goal_tolerance);
+  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal);
 
-  // /**
-  //  * @brief Send a goal for the robot to navigate to
-  //  * @param handle Navigation handle
-  //  * @param order Order message
-  //  * @param goal Target pose in the global frame
-  //  * @param xy_goal_tolerance Acceptable error in X/Y (meters)
-  //  * @param yaw_goal_tolerance Acceptable angular error (radians)
-  //  * @return true if goal was accepted and sent successfully
-  //  */
-  // bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order,
-  //                               const PoseStamped &goal,
-  //                               double xy_goal_tolerance, double yaw_goal_tolerance);
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   * @note If order was obtained from convert2COrder(), call order_free(&order) when done
+   */
+  bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
+
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param marker Marker name or ID (null-terminated string)
    * @param goal Target pose for docking
-   * @param xy_goal_tolerance Acceptable XY error (meters)
-   * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-  bool navigation_dock_to(NavigationHandle handle, const char *marker,
-                          const PoseStamped goal,
-                          double xy_goal_tolerance, double yaw_goal_tolerance);
+  bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal);
+
+  /**
+   * @brief Send a docking goal to a predefined marker
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose for docking
+   * @return true if docking command succeeded
+   */
+  bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
+
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+   bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
-  // /**
-  //  * @brief Send a docking goal to a predefined marker
-  //  * @param handle Navigation handle
-  //  * @param order Order message
-  //  * @param goal Target pose for docking
-  //  * @param xy_goal_tolerance Acceptable XY error (meters)
-  //  * @param yaw_goal_tolerance Acceptable heading error (radians)
-  //  * @return true if docking command succeeded
-  //  */
-  // bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
-  //                               const PoseStamped &goal,
-  //                               double xy_goal_tolerance, double yaw_goal_tolerance);
 
   /**
    * @brief Move straight toward the target position
@@ -226,12 +196,10 @@ extern "C"
   /**
    * @brief Rotate in place to align with target orientation
    * @param handle Navigation handle
-   * @param goal Pose containing desired heading (only Z-axis used)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
+   * @param goal_yaw Desired heading (radians)
    * @return true if rotation command was sent successfully
    */
-  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal,
-                            double yaw_goal_tolerance);
+  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal);
 
   /**
    * @brief Pause the robot's movement

src/APIs/c_api/include/nav_c_api_2.h

@@ -0,0 +1,301 @@
+#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

src/APIs/c_api/include/protocol_msgs/Action.h

@@ -0,0 +1,30 @@
+#ifndef C_API_PROTOCOL_MSGS_ACTION_H
+#define C_API_PROTOCOL_MSGS_ACTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ActionParameter.h"
+
+#define PROTOCOL_MSGS_ACTION_BLOCKING_NONE "NONE"
+#define PROTOCOL_MSGS_ACTION_BLOCKING_SOFT "SOFT"
+#define PROTOCOL_MSGS_ACTION_BLOCKING_HARD "HARD"
+
+typedef struct
+{
+  char *actionType;
+  char *actionId;
+  char *actionDescription;
+  char *blockingType;
+  ActionParameter *actionParameters;
+  size_t actionParameters_count;
+} Action;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ACTION_H

src/APIs/c_api/include/protocol_msgs/ActionParameter.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
+#define C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *key;
+  char *value;
+} ActionParameter;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H

src/APIs/c_api/include/protocol_msgs/ControlPoint.h

@@ -0,0 +1,22 @@
+#ifndef C_API_PROTOCOL_MSGS_CONTROLPOINT_H
+#define C_API_PROTOCOL_MSGS_CONTROLPOINT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double weight;
+} ControlPoint;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_CONTROLPOINT_H

src/APIs/c_api/include/protocol_msgs/Edge.h

@@ -0,0 +1,42 @@
+#ifndef C_API_PROTOCOL_MSGS_EDGE_H
+#define C_API_PROTOCOL_MSGS_EDGE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Trajectory.h"
+#include "protocol_msgs/Action.h"
+
+#define PROTOCOL_MSGS_EDGE_ORIENTATION_GLOBAL "GLOBAL"
+#define PROTOCOL_MSGS_EDGE_ORIENTATION_TANGENTIAL "TANGENTIAL"
+
+typedef struct
+{
+  char *edgeId;
+  int32_t sequenceId;
+  char *edgeDescription;
+  uint8_t released;
+  char *startNodeId;
+  char *endNodeId;
+  double maxSpeed;
+  double maxHeight;
+  double minHeight;
+  double orientation;
+  char *orientationType;
+  char *direction;
+  uint8_t rotationAllowed;
+  double maxRotationSpeed;
+  Trajectory trajectory;
+  double length;
+  Action *actions;
+  size_t actions_count;
+} Edge;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_EDGE_H

src/APIs/c_api/include/protocol_msgs/Error.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_ERROR_H
+#define C_API_PROTOCOL_MSGS_ERROR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ErrorReference.h"
+
+#define PROTOCOL_MSGS_ERROR_LEVEL_WARNING "WARNING"
+#define PROTOCOL_MSGS_ERROR_LEVEL_FATAL "FATAL"
+
+typedef struct
+{
+  char *errorType;
+  ErrorReference *errorReferences;
+  size_t errorReferences_count;
+  char *errorDescription;
+  char *errorLevel;
+} Error;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ERROR_H

src/APIs/c_api/include/protocol_msgs/ErrorReference.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
+#define C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *referenceKey;
+  char *referenceValue;
+} ErrorReference;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ERRORREFERENCE_H

src/APIs/c_api/include/protocol_msgs/Info.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_INFO_H
+#define C_API_PROTOCOL_MSGS_INFO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/InfoReference.h"
+
+#define PROTOCOL_MSGS_INFO_LEVEL_DEBUG "DEBUG"
+#define PROTOCOL_MSGS_INFO_LEVEL_INFO "INFO"
+
+typedef struct
+{
+  char *infoType;
+  InfoReference *infoReferences;
+  size_t infoReferences_count;
+  char *infoDescription;
+  char *infoLevel;
+} Info;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFO_H

src/APIs/c_api/include/protocol_msgs/InfoReference.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_INFOREFERENCE_H
+#define C_API_PROTOCOL_MSGS_INFOREFERENCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *referenceKey;
+  char *referenceValue;
+} InfoReference;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFOREFERENCE_H

src/APIs/c_api/include/protocol_msgs/Information.h

@@ -0,0 +1,22 @@
+#ifndef C_API_PROTOCOL_MSGS_INFORMATION_H
+#define C_API_PROTOCOL_MSGS_INFORMATION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Info.h"
+
+typedef struct
+{
+  Info *information;
+  size_t information_count;
+} Information;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFORMATION_H

src/APIs/c_api/include/protocol_msgs/Node.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_NODE_H
+#define C_API_PROTOCOL_MSGS_NODE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/NodePosition.h"
+#include "protocol_msgs/Action.h"
+
+typedef struct
+{
+  char *nodeId;
+  int32_t sequenceId;
+  char *nodeDescription;
+  uint8_t released;
+  NodePosition nodePosition;
+  Action *actions;
+  size_t actions_count;
+} Node;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_NODE_H

src/APIs/c_api/include/protocol_msgs/NodePosition.h

@@ -0,0 +1,26 @@
+#ifndef C_API_PROTOCOL_MSGS_NODEPOSITION_H
+#define C_API_PROTOCOL_MSGS_NODEPOSITION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double theta;
+  float allowedDeviationXY;
+  float allowedDeviationTheta;
+  char *mapId;
+  char *mapDescription;
+} NodePosition;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_NODEPOSITION_H

src/APIs/c_api/include/protocol_msgs/Order.h

@@ -0,0 +1,33 @@
+#ifndef C_API_PROTOCOL_MSGS_ORDER_H
+#define C_API_PROTOCOL_MSGS_ORDER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Node.h"
+#include "protocol_msgs/Edge.h"
+
+typedef struct
+{
+  int32_t headerId;
+  char *timestamp;
+  char *version;
+  char *manufacturer;
+  char *serialNumber;
+  char *orderId;
+  uint32_t orderUpdateId;
+  Node *nodes;
+  size_t nodes_count;
+  Edge *edges;
+  size_t edges_count;
+  char *zoneSetId;
+} Order;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ORDER_H

src/APIs/c_api/include/protocol_msgs/Trajectory.h

@@ -0,0 +1,25 @@
+#ifndef C_API_PROTOCOL_MSGS_TRAJECTORY_H
+#define C_API_PROTOCOL_MSGS_TRAJECTORY_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ControlPoint.h"
+
+typedef struct
+{
+  uint32_t degree;
+  double *knotVector;
+  size_t knotVector_count;
+  ControlPoint *controlPoints;
+  size_t controlPoints_count;
+} Trajectory;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_TRAJECTORY_H

src/APIs/c_api/include/sensor_msgs/DepthCameraData.h

@@ -0,0 +1,36 @@
+#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

src/APIs/c_api/src/convertor.cpp

@@ -201,6 +201,70 @@ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyG
   }
 }
 
+static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.pose = convert2CPose2D(cpp.pose);
+}
+
+void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.poses_count = cpp.poses.size();
+  out.poses = nullptr;
+  if (out.poses_count > 0)
+  {
+    out.poses = static_cast<Pose2DStamped*>(malloc(out.poses_count * sizeof(Pose2DStamped)));
+    if (out.poses)
+    {
+      for (size_t i = 0; i < out.poses_count; i++)
+        convert2CPose2DStamped(cpp.poses[i], out.poses[i]);
+    }
+  }
+}
+
+void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.x = cpp.x;
+  out.y = cpp.y;
+  out.width = cpp.width;
+  out.height = cpp.height;
+  out.data_count = cpp.data.size();
+  out.data = nullptr;
+  if (out.data_count > 0)
+  {
+    out.data = static_cast<int8_t*>(malloc(out.data_count * sizeof(int8_t)));
+    if (out.data)
+      memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t));
+  }
+}
+
+static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out)
+{
+  out.points_count = cpp.points.size();
+  out.points = nullptr;
+  if (out.points_count > 0)
+  {
+    out.points = static_cast<Point32*>(malloc(out.points_count * sizeof(Point32)));
+    if (out.points)
+    {
+      for (size_t i = 0; i < out.points_count; i++)
+      {
+        out.points[i].x = cpp.points[i].x;
+        out.points[i].y = cpp.points[i].y;
+        out.points[i].z = cpp.points[i].z;
+      }
+    }
+  }
+}
+
+void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  convert2CPolygon(cpp.polygon, out.polygon);
+}
+
 void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out)
 {
   out.header.seq = cpp.header.seq;
@@ -361,3 +425,408 @@ robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c)
     memcpy(cpp.data.data(), c.data, c.data_count);
   return cpp;
 }
+
+// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) ---
+
+static inline char* dup_cstr(const char* s)
+{
+  if (!s) return nullptr;
+  const size_t n = std::strlen(s) + 1;
+  char* p = static_cast<char*>(std::malloc(n));
+  if (p) std::memcpy(p, s, n);
+  return p;
+}
+
+static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c)
+{
+  robot_protocol_msgs::ActionParameter cpp;
+  cpp.key = c.key ? c.key : "";
+  cpp.value = c.value ? c.value : "";
+  return cpp;
+}
+
+static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp)
+{
+  ActionParameter c;
+  c.key = dup_cstr(cpp.key.c_str());
+  c.value = dup_cstr(cpp.value.c_str());
+  return c;
+}
+
+static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c)
+{
+  robot_protocol_msgs::ControlPoint cpp;
+  cpp.x = c.x;
+  cpp.y = c.y;
+  cpp.weight = c.weight;
+  return cpp;
+}
+
+static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp)
+{
+  ControlPoint c;
+  c.x = cpp.x;
+  c.y = cpp.y;
+  c.weight = cpp.weight;
+  return c;
+}
+
+static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c)
+{
+  robot_protocol_msgs::NodePosition cpp;
+  cpp.x = c.x;
+  cpp.y = c.y;
+  cpp.theta = c.theta;
+  cpp.allowedDeviationXY = c.allowedDeviationXY;
+  cpp.allowedDeviationTheta = c.allowedDeviationTheta;
+  cpp.mapId = c.mapId ? c.mapId : "";
+  cpp.mapDescription = c.mapDescription ? c.mapDescription : "";
+  return cpp;
+}
+
+static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp)
+{
+  NodePosition c;
+  c.x = cpp.x;
+  c.y = cpp.y;
+  c.theta = cpp.theta;
+  c.allowedDeviationXY = cpp.allowedDeviationXY;
+  c.allowedDeviationTheta = cpp.allowedDeviationTheta;
+  c.mapId = dup_cstr(cpp.mapId.c_str());
+  c.mapDescription = dup_cstr(cpp.mapDescription.c_str());
+  return c;
+}
+
+static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c)
+{
+  robot_protocol_msgs::Trajectory cpp;
+  cpp.degree = c.degree;
+  const size_t kv = c.knotVector ? c.knotVector_count : 0;
+  cpp.knotVector.resize(kv);
+  for (size_t i = 0; i < kv; i++)
+    cpp.knotVector[i] = c.knotVector[i];
+  const size_t ncp = c.controlPoints ? c.controlPoints_count : 0;
+  cpp.controlPoints.resize(ncp);
+  for (size_t i = 0; i < ncp; i++)
+    cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]);
+  return cpp;
+}
+
+static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp)
+{
+  Trajectory c;
+  c.degree = cpp.degree;
+  c.knotVector_count = cpp.knotVector.size();
+  c.knotVector = c.knotVector_count ? static_cast<double*>(std::malloc(c.knotVector_count * sizeof(double))) : nullptr;
+  if (c.knotVector)
+    for (size_t i = 0; i < c.knotVector_count; i++)
+      c.knotVector[i] = cpp.knotVector[i];
+  c.controlPoints_count = cpp.controlPoints.size();
+  c.controlPoints = c.controlPoints_count ? static_cast<ControlPoint*>(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr;
+  if (c.controlPoints)
+    for (size_t i = 0; i < c.controlPoints_count; i++)
+      c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Action convert2CppAction(const Action& c)
+{
+  robot_protocol_msgs::Action cpp;
+  cpp.actionType = c.actionType ? c.actionType : "";
+  cpp.actionId = c.actionId ? c.actionId : "";
+  cpp.actionDescription = c.actionDescription ? c.actionDescription : "";
+  cpp.blockingType = c.blockingType ? c.blockingType : "";
+  const size_t n = c.actionParameters ? c.actionParameters_count : 0;
+  cpp.actionParameters.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]);
+  return cpp;
+}
+
+static Action convert2CAction(const robot_protocol_msgs::Action& cpp)
+{
+  Action c;
+  c.actionType = dup_cstr(cpp.actionType.c_str());
+  c.actionId = dup_cstr(cpp.actionId.c_str());
+  c.actionDescription = dup_cstr(cpp.actionDescription.c_str());
+  c.blockingType = dup_cstr(cpp.blockingType.c_str());
+  c.actionParameters_count = cpp.actionParameters.size();
+  c.actionParameters = c.actionParameters_count ? static_cast<ActionParameter*>(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr;
+  if (c.actionParameters)
+    for (size_t i = 0; i < c.actionParameters_count; i++)
+      c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Node convert2CppNode(const Node& c)
+{
+  robot_protocol_msgs::Node cpp;
+  cpp.nodeId = c.nodeId ? c.nodeId : "";
+  cpp.sequenceId = c.sequenceId;
+  cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : "";
+  cpp.released = c.released;
+  cpp.nodePosition = convert2CppNodePosition(c.nodePosition);
+  const size_t n = c.actions ? c.actions_count : 0;
+  cpp.actions.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actions[i] = convert2CppAction(c.actions[i]);
+  return cpp;
+}
+
+static Node convert2CNode(const robot_protocol_msgs::Node& cpp)
+{
+  Node c;
+  c.nodeId = dup_cstr(cpp.nodeId.c_str());
+  c.sequenceId = cpp.sequenceId;
+  c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str());
+  c.released = cpp.released;
+  c.nodePosition = convert2CNodePosition(cpp.nodePosition);
+  c.actions_count = cpp.actions.size();
+  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
+  if (c.actions)
+    for (size_t i = 0; i < c.actions_count; i++)
+      c.actions[i] = convert2CAction(cpp.actions[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c)
+{
+  robot_protocol_msgs::Edge cpp;
+  cpp.edgeId = c.edgeId ? c.edgeId : "";
+  cpp.sequenceId = c.sequenceId;
+  cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : "";
+  cpp.released = c.released;
+  cpp.startNodeId = c.startNodeId ? c.startNodeId : "";
+  cpp.endNodeId = c.endNodeId ? c.endNodeId : "";
+  cpp.maxSpeed = c.maxSpeed;
+  cpp.maxHeight = c.maxHeight;
+  cpp.minHeight = c.minHeight;
+  cpp.orientation = c.orientation;
+  cpp.orientationType = c.orientationType ? c.orientationType : "";
+  cpp.direction = c.direction ? c.direction : "";
+  cpp.rotationAllowed = c.rotationAllowed;
+  cpp.maxRotationSpeed = c.maxRotationSpeed;
+  cpp.trajectory = convert2CppTrajectory(c.trajectory);
+  cpp.length = c.length;
+  const size_t n = c.actions ? c.actions_count : 0;
+  cpp.actions.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actions[i] = convert2CppAction(c.actions[i]);
+  return cpp;
+}
+
+static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp)
+{
+  Edge c;
+  c.edgeId = dup_cstr(cpp.edgeId.c_str());
+  c.sequenceId = cpp.sequenceId;
+  c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str());
+  c.released = cpp.released;
+  c.startNodeId = dup_cstr(cpp.startNodeId.c_str());
+  c.endNodeId = dup_cstr(cpp.endNodeId.c_str());
+  c.maxSpeed = cpp.maxSpeed;
+  c.maxHeight = cpp.maxHeight;
+  c.minHeight = cpp.minHeight;
+  c.orientation = cpp.orientation;
+  c.orientationType = dup_cstr(cpp.orientationType.c_str());
+  c.direction = dup_cstr(cpp.direction.c_str());
+  c.rotationAllowed = cpp.rotationAllowed;
+  c.maxRotationSpeed = cpp.maxRotationSpeed;
+  c.trajectory = convert2CTrajectory(cpp.trajectory);
+  c.length = cpp.length;
+  c.actions_count = cpp.actions.size();
+  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
+  if (c.actions)
+    for (size_t i = 0; i < c.actions_count; i++)
+      c.actions[i] = convert2CAction(cpp.actions[i]);
+  return c;
+}
+
+static void free_action_parameter(ActionParameter* p)
+{
+  if (!p) return;
+  std::free(p->key);
+  std::free(p->value);
+  p->key = nullptr;
+  p->value = nullptr;
+}
+
+static void free_action(Action* p)
+{
+  if (!p) return;
+  std::free(p->actionType);
+  std::free(p->actionId);
+  std::free(p->actionDescription);
+  std::free(p->blockingType);
+  p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr;
+  if (p->actionParameters) {
+    for (size_t i = 0; i < p->actionParameters_count; i++)
+      free_action_parameter(&p->actionParameters[i]);
+    std::free(p->actionParameters);
+    p->actionParameters = nullptr;
+  }
+  p->actionParameters_count = 0;
+}
+
+static void free_node_position(NodePosition* p)
+{
+  if (!p) return;
+  std::free(p->mapId);
+  std::free(p->mapDescription);
+  p->mapId = p->mapDescription = nullptr;
+}
+
+static void free_trajectory(Trajectory* p)
+{
+  if (!p) return;
+  std::free(p->knotVector);
+  p->knotVector = nullptr;
+  p->knotVector_count = 0;
+  std::free(p->controlPoints);
+  p->controlPoints = nullptr;
+  p->controlPoints_count = 0;
+}
+
+static void free_node(Node* p)
+{
+  if (!p) return;
+  std::free(p->nodeId);
+  std::free(p->nodeDescription);
+  p->nodeId = p->nodeDescription = nullptr;
+  free_node_position(&p->nodePosition);
+  if (p->actions) {
+    for (size_t i = 0; i < p->actions_count; i++)
+      free_action(&p->actions[i]);
+    std::free(p->actions);
+    p->actions = nullptr;
+  }
+  p->actions_count = 0;
+}
+
+static void free_edge(Edge* p)
+{
+  if (!p) return;
+  std::free(p->edgeId);
+  std::free(p->edgeDescription);
+  std::free(p->startNodeId);
+  std::free(p->endNodeId);
+  std::free(p->orientationType);
+  std::free(p->direction);
+  p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr;
+  p->orientationType = p->direction = nullptr;
+  free_trajectory(&p->trajectory);
+  if (p->actions) {
+    for (size_t i = 0; i < p->actions_count; i++)
+      free_action(&p->actions[i]);
+    std::free(p->actions);
+    p->actions = nullptr;
+  }
+  p->actions_count = 0;
+}
+
+robot_protocol_msgs::Order convert2CppOrder(const Order& order)
+{
+  robot_protocol_msgs::Order cpp_order;
+  cpp_order.headerId = order.headerId;
+  cpp_order.timestamp = order.timestamp ? order.timestamp : "";
+  cpp_order.version = order.version ? order.version : "";
+  cpp_order.manufacturer = order.manufacturer ? order.manufacturer : "";
+  cpp_order.serialNumber = order.serialNumber ? order.serialNumber : "";
+  cpp_order.orderId = order.orderId ? order.orderId : "";
+  cpp_order.orderUpdateId = order.orderUpdateId;
+  const size_t nn = order.nodes ? order.nodes_count : 0;
+  cpp_order.nodes.resize(nn);
+  for (size_t i = 0; i < nn; i++)
+    cpp_order.nodes[i] = convert2CppNode(order.nodes[i]);
+  const size_t ne = order.edges ? order.edges_count : 0;
+  cpp_order.edges.resize(ne);
+  for (size_t i = 0; i < ne; i++)
+    cpp_order.edges[i] = convert2CppEdge(order.edges[i]);
+  cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : "";
+  return cpp_order;
+}
+
+Order convert2COrder(const robot_protocol_msgs::Order& cpp_order)
+{
+  Order order;
+  order.headerId = cpp_order.headerId;
+  order.timestamp = dup_cstr(cpp_order.timestamp.c_str());
+  order.version = dup_cstr(cpp_order.version.c_str());
+  order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str());
+  order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str());
+  order.orderId = dup_cstr(cpp_order.orderId.c_str());
+  order.orderUpdateId = cpp_order.orderUpdateId;
+  order.nodes_count = cpp_order.nodes.size();
+  order.nodes = order.nodes_count ? static_cast<Node*>(std::malloc(order.nodes_count * sizeof(Node))) : nullptr;
+  if (order.nodes)
+    for (size_t i = 0; i < order.nodes_count; i++)
+      order.nodes[i] = convert2CNode(cpp_order.nodes[i]);
+  order.edges_count = cpp_order.edges.size();
+  order.edges = order.edges_count ? static_cast<Edge*>(std::malloc(order.edges_count * sizeof(Edge))) : nullptr;
+  if (order.edges)
+    for (size_t i = 0; i < order.edges_count; i++)
+      order.edges[i] = convert2CEdge(cpp_order.edges[i]);
+  order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str());
+  return order;
+}
+
+extern "C" void order_free(Order* order)
+{
+  if (!order) return;
+  std::free(order->timestamp);
+  std::free(order->version);
+  std::free(order->manufacturer);
+  std::free(order->serialNumber);
+  std::free(order->orderId);
+  std::free(order->zoneSetId);
+  order->timestamp = order->version = order->manufacturer = nullptr;
+  order->serialNumber = order->orderId = order->zoneSetId = nullptr;
+  if (order->nodes) {
+    for (size_t i = 0; i < order->nodes_count; i++)
+      free_node(&order->nodes[i]);
+    std::free(order->nodes);
+    order->nodes = nullptr;
+  }
+  order->nodes_count = 0;
+  if (order->edges) {
+    for (size_t i = 0; i < order->edges_count; i++)
+      free_edge(&order->edges[i]);
+    std::free(order->edges);
+    order->edges = nullptr;
+  }
+  order->edges_count = 0;
+}
+
+bool isQuaternionValid(const Quaternion& q)
+{
+  // first we need to check if the quaternion has nan's or infs
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
+    return false;
+  }
+
+  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
+
+  // next, we need to check if the length of the quaternion is close to zero
+  if (tf_q.length2() < 1e-6)
+  {
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
+    return false;
+  }
+
+  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
+  tf_q.normalize();
+
+  tf3::Vector3 up(0, 0, 1);
+
+  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
+
+  if (fabs(dot - 1) > 1e-3)
+  {
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
+    return false;
+  }
+
+  return true;
+}

src/APIs/c_api/src/convertor_2.cpp

@@ -0,0 +1,78 @@
+#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;
+}

src/APIs/c_api/src/nav_c_api_2.cpp

@@ -0,0 +1,422 @@
+/**
+ * @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;
+}

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

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

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

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

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

@@ -15,9 +15,9 @@ namespace mkt_algorithm
     class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
     {
     public:
-      GoStraight() {};
+      GoStraight();
 
-      virtual ~GoStraight() {};
+      virtual ~GoStraight();
 
       /**
        * @brief Initialize parameters as needed

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

@@ -30,9 +30,14 @@ namespace mkt_algorithm
     class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
     {
     public:
-      PredictiveTrajectory() : initialized_(false), nav_stop_(false),
-          near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {};
+      /**
+       * @brief Constructor
+       */
+      PredictiveTrajectory();
 
+      /**
+       * @brief Destructor
+       */
       virtual ~PredictiveTrajectory();
 
       // Standard ScoreAlgorithm Interface
@@ -176,7 +181,7 @@ namespace mkt_algorithm
        */
       robot_nav_2d_msgs::Path2D generateTrajectory(  
         const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
-        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
+        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);
 
       /**
        * @brief Generate trajectory
@@ -189,11 +194,11 @@ namespace mkt_algorithm
 
       /**
        * @brief Generate Hermite trajectory
-       * @param pose
+       * @param path
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Generate Hermite quadratic trajectory
@@ -201,7 +206,7 @@ namespace mkt_algorithm
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Should rotate to path
@@ -388,13 +393,8 @@ namespace mkt_algorithm
       double near_goal_heading_integral_;
       double near_goal_heading_last_error_;
       bool near_goal_heading_was_active_;
-
-      // Regulated linear velocity scaling
-      bool use_regulated_linear_velocity_scaling_;
-
+      
       double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
 
       bool use_cost_regulated_linear_velocity_scaling_;
       double inflation_cost_scaling_factor_;
@@ -412,6 +412,14 @@ namespace mkt_algorithm
       Eigen::MatrixXd Q;
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
+
+      // Kalman filter tuning (for v and w filtering)
+      double kf_q_v_;
+      double kf_q_w_;
+      double kf_r_v_;
+      double kf_r_w_;
+      double kf_p0_;
+      bool kf_filter_angular_;
 #ifdef BUILD_WITH_ROS
       ros::Publisher lookahead_point_pub_;
 #endif

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

@@ -14,9 +14,9 @@ namespace mkt_algorithm
     class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
     {
     public:
-      RotateToGoal() {};
+      RotateToGoal();
 
-      virtual ~RotateToGoal() {};
+      virtual ~RotateToGoal();
 
       /**
        * @brief Initialize parameters as needed

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

@@ -2,6 +2,13 @@
 #include <boost/dll/alias.hpp>
 #include <robot/robot.h>
 
+mkt_algorithm::diff::GoStraight::GoStraight()
+  : PredictiveTrajectory()
+{
+}
+
+mkt_algorithm::diff::GoStraight::~GoStraight() {}
+
 void mkt_algorithm::diff::GoStraight::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
@@ -85,6 +92,7 @@ void mkt_algorithm::diff::GoStraight::initialize(
 mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
+  // robot::log_info("x %f y %f theta %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   mkt_msgs::Trajectory2D result;
   result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
   if (!traj_)
@@ -123,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)
@@ -143,8 +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;
-    robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", transformed_plan.poses.front().pose.y, transformed_plan.poses.front().pose.theta);
-    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+    transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
     // Normal Pure Pursuit
@@ -157,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     sign_x, 
     dt, 
     drive_cmd);
-  }
+  // }
   applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
 
   if (this->nav_stop_)

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

@@ -1,6 +1,51 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
 #include <boost/dll/alias.hpp>
 
+
+mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
+  : initialized_(false),
+    nav_stop_(false),
+    x_direction_(0.0),
+    y_direction_(0.0),
+    theta_direction_(0.0),
+    use_velocity_scaled_lookahead_dist_(false),
+    lookahead_time_(0.0),
+    lookahead_dist_(0.0),
+    min_lookahead_dist_(0.0),
+    max_lookahead_dist_(0.0),
+    max_lateral_accel_(0.0),
+    use_rotate_to_heading_(false),
+    rotate_to_heading_min_angle_(0.0),
+    min_path_distance_(0.0),
+    max_path_distance_(0.0),
+    use_final_heading_alignment_(false),
+    final_heading_xy_tolerance_(0.0),
+    final_heading_angle_tolerance_(0.0),
+    final_heading_min_velocity_(0.0),
+    final_heading_kp_angular_(0.0),
+    final_heading_ki_angular_(0.0),
+    final_heading_kd_angular_(0.0),
+    near_goal_heading_integral_(0.0),
+    near_goal_heading_last_error_(0.0),
+    near_goal_heading_was_active_(false),
+    min_approach_linear_velocity_(0.0),
+    use_cost_regulated_linear_velocity_scaling_(false),
+    inflation_cost_scaling_factor_(0.0),
+    cost_scaling_dist_(0.0),
+    cost_scaling_gain_(0.0),
+    control_duration_(0.0),
+    kf_(nullptr),
+    kf_q_v_(0.25),
+    kf_q_w_(0.8),
+    kf_r_v_(0.05),
+    kf_r_w_(0.08),
+    kf_p0_(0.5),
+    kf_filter_angular_(false),
+    m_(0),
+    n_(0)
+{
+}
+
 mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
@@ -44,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     // kalman
     last_actuator_update_ = robot::Time::now();
-    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
-    m_ = 2; // measurements: x, y, theta
+    // State: [v, a, j, w, alpha, jw] where:
+    //  - v: linear velocity, a: linear accel, j: linear jerk
+    //  - w: angular velocity, alpha: angular accel, jw: angular jerk
+    // Measurement: [v, w]
+    n_ = 6;
+    m_ = 2;
     double dt = control_duration_;
 
     // Khởi tạo ma trận
     A = Eigen::MatrixXd::Identity(n_, n_);
     C = Eigen::MatrixXd::Zero(m_, n_);
     Q = Eigen::MatrixXd::Zero(n_, n_);
-    R = Eigen::MatrixXd::Identity(m_, m_);
-    P = Eigen::MatrixXd::Identity(n_, n_);
+    R = Eigen::MatrixXd::Zero(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
 
     for (int i = 0; i < n_; i += 3)
     {
@@ -64,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     C(0, 0) = 1;
     C(1, 3) = 1;
-    Q(2, 2) = 0.1;
-    Q(5, 5) = 0.6;
+    Q(2, 2) = std::max(1e-12, kf_q_v_);
+    Q(5, 5) = std::max(1e-12, kf_q_w_);
 
-    R(0, 0) = 0.1;
-    R(1, 1) = 0.2;
-
-    P(3, 3) = 0.4;
-    P(4, 4) = 0.4;
-    P(5, 5) = 0.4;
+    R(0, 0) = std::max(1e-12, kf_r_v_);
+    R(1, 1) = std::max(1e-12, kf_r_w_);
 
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
@@ -127,11 +172,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
     trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
   // Inflation cost scaling (Limit velocity by proximity to obstacles)
   nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
   nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
@@ -143,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
+
+  // Kalman filter tuning (filtering v and w commands)
+  nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
+  nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
+  nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
+  nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
+  nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
+  nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);
+
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
   control_duration_ = 1.0 / control_frequency;
 
@@ -163,6 +212,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
+    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+
     traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
   }
 }
@@ -310,6 +368,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
+
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -346,42 +405,47 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     
   }
-  else
+  else if(compute_plan_.poses.size() == 1)
   {
     try
     {
-      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
-      auto 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());
-      const double dir_path = 0.0;
+
+      
+      auto goal_pose = compute_plan_.poses.front().pose;
+      auto start_pose = pose.pose;
+      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
+      double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
     catch (std::exception &e)
     {
-      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
-      return false;
+      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
+      x_direction = x_direction_; 
     }
   }
 
@@ -419,6 +483,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
   drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  // drive_cmd.x = sqrt(twist.x * twist.x);
+  
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
@@ -454,8 +520,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
+  // 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;
 
-  double angle_to_heading;
+  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);
+
+  double angle_to_heading; 
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
     if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
@@ -470,27 +540,23 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    // === Final Heading Alignment Check ===
-    double xy_error = 0.0, heading_error = 0.0;
-    if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
-    {
-      // Use Arc Motion controller for final heading alignment
-      alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
-#ifdef BUILD_WITH_ROS
-      ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                        heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
-    }
-    else
-    {
-      // if(fabs(carrot_pose.pose.y) > 0.2)
-      // {
-      //   lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
-      // }
-      robot_nav_2d_msgs::Twist2D drive_target;
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+//     // === Final Heading Alignment Check ===
+//     double xy_error = 0.0, heading_error = 0.0;
+//     if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
+//     {
+//       // Use Arc Motion controller for final heading alignment
+//       alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+// #ifdef BUILD_WITH_ROS
+//       ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//                         heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+// #endif
+//     }
+//     else
+//     {
+      robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
+      
       // Normal Pure Pursuit
       this->computePurePursuit(
       carrot_pose, 
@@ -501,9 +567,8 @@ 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_)
     {
       if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
@@ -516,27 +581,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
-
-    // Eigen::VectorXd y(2);
-    // y << drive_cmd.x, drive_cmd.theta;
-
-    // // Cập nhật lại A nếu dt thay đổi
-    // for (int i = 0; i < n_; ++i)
-    //   for (int j = 0; j < n_; ++j)
-    //     A(i, j) = (i == j ? 1.0 : 0.0);
-    // for (int i = 0; i < n_; i += 3)
-    // {
-    //   A(i, i + 1) = dt;
-    //   A(i, i + 2) = 0.5 * dt * dt;
-    //   A(i + 1, i + 2) = dt;
-    // }
-    // kf_->update(y, dt, A);
-    // double v_min = min_approach_linear_velocity_;
-    // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
-    // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
+    
   }
-
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
@@ -548,6 +594,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     break;
   }
 
+  if(fabs(v_max == 0.0))
+  {
+    drive_cmd.x = 0.0;
+    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
+    return result;
+  }
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -565,7 +617,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 {
   // 1) Curvature from pure pursuit
   const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  const double L2_min = 0.05;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_min = 0.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
   const double L2_safe = std::max(L2, L2_min);
   const double L = std::sqrt(L2_safe);
 
@@ -573,16 +625,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   // 3) Adjust speed using Hermite trajectory curvature + remaining distance
   double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
-  const double L_min = 0.1;  // m, chỉnh theo nhu cầu
-  double scale_close = std::clamp(L / L_min, 0.0, 1.0);
-  v_target *= scale_close;
-
+  // const double L_min = 0.1;  // m, chỉnh theo nhu cầu
+  // double scale_close = std::clamp(L / L_min, 0.0, 1.0);
+  // v_target *= scale_close;
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
   {
     double scale = y_soft / y_abs;  // y càng lớn => scale càng nhỏ
-    scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
+    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
     v_target *= scale;
     robot_nav_2d_msgs::Twist2D cmd, result;
     cmd.x = v_target;
@@ -593,7 +644,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   // 4) Maintain minimum approach speed
   if (std::fabs(v_target) < min_approach_linear_velocity)
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
-
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
@@ -604,18 +654,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       for(int i = trajectory.poses.size() - 2; i >= 0; i--)
       {
         const auto& p = trajectory.poses[i].pose;
-        if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
+        const auto& dx =  p1.x - p.x ;
+        const auto& dy =  p1.y - p.y ;
+        if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
         {
-          heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
+          if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+            continue;
+          heading_ref = std::atan2(dy, dx);
           if(sign_x < 0.0)
-          {
-            heading_ref = angles::normalize_angle(M_PI + heading_ref);
-          }
+          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
           break;
         }
       }
 
-      const double error = angles::normalize_angle(heading_ref);
+      const double error = heading_ref;
       double w_heading = 0.0;
       pid(error, 
         near_goal_heading_integral_, 
@@ -628,10 +680,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       // Apply acceleration limits
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
       w_target = velocity.theta + dw_heading;
+      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
     else
     {
-      w_target = 0.0;
+      w_target = std::clamp(w_target, -0.001, 0.001);
       near_goal_heading_was_active_ = false;
     }
   }
@@ -647,6 +700,27 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
+  
+  Eigen::VectorXd y(2);
+  y << drive_cmd.x, drive_cmd.theta;
+
+  // Cập nhật lại A nếu dt thay đổi
+  for (int i = 0; i < n_; ++i)
+    for (int j = 0; j < n_; ++j)
+      A(i, j) = (i == j ? 1.0 : 0.0);
+  for (int i = 0; i < n_; i += 3)
+  {
+    A(i, i + 1) = dt;
+    A(i, i + 2) = 0.5 * dt * dt;
+    A(i + 1, i + 2) = dt;
+  }
+  kf_->update(y, dt, A);
+  double v_min = min_approach_linear_velocity_;
+  drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
+  drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
+  if (kf_filter_angular_)
+    drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
+  // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -670,8 +744,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
     double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
     target_speed = max_speed * cosine_factor;
   }
-
-  double reduce_speed = std::min(max_speed, min_speed_xy_);
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
+  double reduce_speed = std::min(max_speed, v_min);
   if (s < S_final)
   {
     double r = std::clamp(s / S_final, 0.0, 1.0);
@@ -704,21 +779,29 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   // const double max_kappa = calculateMaxKappa(global_plan);
   // const bool curvature = max_kappa > straight_threshold;
   double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-  if(is_stopped && global_plan.poses.size() >= 2)
+  if(is_stopped && global_plan.poses.size() >= 4 &&
+  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
   {
-    const auto& p1 = global_plan.poses[1];
-    for(int i = 2; i < global_plan.poses.size(); i++)
+    const auto& p1 = global_plan.poses[2];
+    for(int i = 3; i < global_plan.poses.size(); i++)
     {
       const auto& p = global_plan.poses[i];
-      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = p.pose.x - p1.pose.x;
+      const auto& dy = p.pose.y - p1.pose.y;
+      if(std::hypot(dx, dy) > costmap_robot_->getCostmap()->getResolution())
       {
-        path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x);
+        if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
+          continue;
+        path_angle = std::atan2(dy, dx);
         break;
       }
     }
   }
   
   // Whether we should rotate robot to rough path heading
+  // if(sign_x < 0.0)
+  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  // angle_to_path = path_angle;
   angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
@@ -735,12 +818,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
 #ifdef BUILD_WITH_ROS
   if (result)
     ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-
-  // else if(fabs(velocity.x) < min_speed_xy_)
-  // {
-  //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
-  //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
-  // }
+#else
+  if (result)
+    robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 #endif
   return result;
 }
@@ -823,13 +903,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
     for(int i = trajectory.poses.size() - 2; i >= 0; i--)
     {
       const auto& p = trajectory.poses[i].pose;
-      if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+      const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+      if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
       {
-        heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
-        if(sign_x < 0.0)
-        {
-          heading_error = angles::normalize_angle(M_PI + heading_error);
-        }
+        heading_error = angles::normalize_angle(std::atan2(dy, dx));
         break;
       }
     }
@@ -879,8 +957,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   // --- Linear velocity calculation ---
   // Base velocity proportional to distance, with minimum for smooth motion
   double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   v_base = std::max(v_base, final_heading_min_velocity_);
-  v_base = std::min(v_base, min_speed_xy_);
+  v_base = std::min(v_base, v_min);
 
   // Scale down when heading error is large (prioritize rotation)
   double heading_scale = 1.0;
@@ -952,7 +1032,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   cmd_vel.theta = omega_current + domega;
 
   // --- Apply velocity limits ---
-  cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_);
+  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
   cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);
 
   // --- Safety: ensure we can stop ---
@@ -1134,9 +1214,11 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   double v_limit = std::fabs(v_target);
   double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
   
+  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
   if (journey_distance < min_journey_squared_)
   {
-    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
   }
 
   if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
@@ -1146,7 +1228,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   }
 
   if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
-    v_limit = min_speed_xy_ * sign_x;
+    v_limit = v_min * sign_x;
 
   if (fabs(decel_lim_x_) > 1e-6)
   {
@@ -1154,6 +1236,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
     const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
     v_limit = std::min(v_limit, v_stop);
   }
+  v_limit = std::min(v_limit, fabs(v_target));
 
   return std::copysign(v_limit, sign_x);
 }
@@ -1163,7 +1246,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   const robot_nav_2d_msgs::Twist2D &drive_target,
   const robot_nav_2d_msgs::Twist2D &velocity,
   const double &sign_x,
-  robot_nav_2d_msgs::Twist2D &drive_cmd)
+  robot_nav_2d_msgs::Twist2D &drive_cmd, 
+  const double &dt)
 {
   if (path.poses.empty())
   {
@@ -1171,24 +1255,31 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
+
+  drive_cmd.x = drive_target.x;
+  drive_cmd.theta = max_vel_theta_;
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
-  drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
-  drive_cmd.theta = max_vel_theta_;
-
-  if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
+  // nếu đường thẳng
+  if (max_kappa <= straight_threshold) 
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
+    if(fabs(path.poses.back().pose.x) < min_lookahead_dist_ * 0.8)
     {
+      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
+        drive_cmd.theta = 0.01;
       return generateParallelPath(path, sign_x);
     }
-    return generateHermiteTrajectory(path.poses.back(), sign_x);
+    return generateHermiteTrajectory(path, sign_x);
   }
   else // nếu đường cong
   {
-    if(fabs(drive_cmd.x) < min_speed_xy_)
-      drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
-    return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
+    const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
+    const double v_min = std::min(fabs(v_limited), min_speed_xy_);
+    if(fabs(drive_cmd.x) < v_min) 
+    {
+      drive_cmd.x = std::copysign(v_min, sign_x);
+    }
+    return generateHermiteQuadraticTrajectory(path, sign_x);
   }
 }
 
@@ -1218,40 +1309,47 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
       dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
       dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
     }
-
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double theta = atan2(dy, dx);
     double x_off = p.x - offset_y * sin(theta)*sign_x;
     double y_off = p.y - offset_y * cos(theta)*sign_x;
 
+    parallel_path.poses[i].header = path.poses[i].header;
     parallel_path.poses[i].pose.x = x_off;
     parallel_path.poses[i].pose.y = y_off;
-    parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
-    parallel_path.poses[i].header = path.poses[i].header;
+    parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
   }
-
   return parallel_path;
 }
 
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D hermite_trajectory;
   hermite_trajectory.poses.clear();
-  hermite_trajectory.header.stamp = pose.header.stamp;
-  hermite_trajectory.header.frame_id = pose.header.frame_id;
+  hermite_trajectory.header = path.header;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = pose.pose.theta;
+  if (path.poses.empty())
+    return hermite_trajectory;
+
+  const auto &goal = path.poses.back();
+  if (hermite_trajectory.header.frame_id.empty())
+    hermite_trajectory.header.frame_id = goal.header.frame_id;
+  if (hermite_trajectory.header.stamp.isZero())
+    hermite_trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  double theta = goal.pose.theta;
   const double L = std::hypot(x, y);
-
   if (L < 1e-6) {
     robot_nav_2d_msgs::Pose2DStamped pose_stamped;
-    pose_stamped.pose.x = 0.0;
-    pose_stamped.pose.y = 0.0;
-    pose_stamped.pose.theta = 0.0;
-    pose_stamped.header.stamp = pose.header.stamp;
-    pose_stamped.header.frame_id = pose.header.frame_id;
+    pose_stamped.pose.x = x;
+    pose_stamped.pose.y = y;
+    pose_stamped.pose.theta = theta;
+    pose_stamped.header.stamp = hermite_trajectory.header.stamp;
+    pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
     hermite_trajectory.poses.push_back(pose_stamped);
     return hermite_trajectory;
   }
@@ -1285,30 +1383,39 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
     double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
 
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
-    robot_nav_2d_msgs::Pose2DStamped pose;
-    pose.pose.x = px;
-    pose.pose.y = py;
-    pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
-    pose.header.stamp = hermite_trajectory.header.stamp;
-    pose.header.frame_id = hermite_trajectory.header.frame_id;
-    hermite_trajectory.poses.push_back(pose);
+    robot_nav_2d_msgs::Pose2DStamped pose_out;
+    pose_out.pose.x = px;
+    pose_out.pose.y = py;
+    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
+    pose_out.header.stamp = hermite_trajectory.header.stamp;
+    pose_out.header.frame_id = hermite_trajectory.header.frame_id;
+    hermite_trajectory.poses.push_back(pose_out);
   }
   return hermite_trajectory;
 }
 
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D trajectory;
   trajectory.poses.clear();
-  trajectory.header.stamp = pose.header.stamp;
-  trajectory.header.frame_id = pose.header.frame_id;
+  trajectory.header = path.header;
+  if (path.poses.empty())
+    return trajectory;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
+  const auto &goal = path.poses.back();
+  if (trajectory.header.frame_id.empty())
+    trajectory.header.frame_id = goal.header.frame_id;
+  if (trajectory.header.stamp.isZero())
+    trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta;
   const double L = std::hypot(x, y);
   if (L < 1e-6)
   {
@@ -1348,6 +1455,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
 
     double dx = 2.0 * ax * t + bx;
     double dy = 2.0 * ay * t + by;
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
     robot_nav_2d_msgs::Pose2DStamped pose_out;

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

@@ -4,6 +4,14 @@
 #include <boost/dll/alias.hpp>
 #include <angles/angles.h>
 
+
+mkt_algorithm::diff::RotateToGoal::RotateToGoal()
+  : PredictiveTrajectory()
+{
+}
+
+mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {}
+
 void mkt_algorithm::diff::RotateToGoal::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h

@@ -15,6 +15,7 @@ namespace mkt_plugins
   {
   public:
     KinematicParameters();
+    virtual ~KinematicParameters();
     void initialize(const robot::NodeHandle &nh);
 
     /**

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h

@@ -12,6 +12,15 @@ namespace mkt_plugins
 class LimitedAccelGenerator : public StandardTrajectoryGenerator
 {
 public:
+  /**
+   * @brief Constructor
+   */
+  LimitedAccelGenerator();
+  /**
+   * @brief Destructor
+   */
+  virtual ~LimitedAccelGenerator();
+  
   /**
    * @brief Initialize the generator with parameters from NodeHandle
    * @param nh NodeHandle for loading acceleration_time parameter

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h

@@ -17,6 +17,16 @@ namespace mkt_plugins
   class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator
   {
   public:
+
+    /**
+     * @brief Constructor
+     */
+    StandardTrajectoryGenerator();
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~StandardTrajectoryGenerator();
     /**
      * @brief Initialize the trajectory generator with parameters from NodeHandle
      * @param nh NodeHandle for loading configuration parameters

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h

@@ -21,7 +21,12 @@ namespace mkt_plugins
      * @brief Default constructor
      * Initializes all iterators to nullptr
      */
-    XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
+    XYThetaIterator();
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~XYThetaIterator();
     
     /**
      * @brief Initialize the iterator with parameters and kinematics

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -7,7 +7,12 @@
 #include <angles/angles.h>
 #include <cmath>
 
-mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25)
+mkt_plugins::GoalChecker::GoalChecker()
+  : nh_(),
+    line_generator_(nullptr),
+    yaw_goal_tolerance_(0.25),
+    xy_goal_tolerance_(0.25),
+    old_xy_goal_tolerance_(0.0)
 {
 }
 
@@ -65,9 +70,8 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
       double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
       if(fabs(tolerance) <= xy_goal_tolerance_) 
       {
-        robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
-        robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
-        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }

src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp

@@ -32,12 +32,42 @@ namespace mkt_plugins
     nh.setParam(decel_param, -accel);
   }
 
-  KinematicParameters::KinematicParameters() : xytheta_direct_(),
-                                               min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
-                                               min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
-                                               acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
-                                               decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
-                                               min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0)
+  KinematicParameters::KinematicParameters()
+    : xytheta_direct_(),
+      min_vel_x_(0.0),
+      min_vel_y_(0.0),
+      max_vel_x_(0.0),
+      max_vel_y_(0.0),
+      max_vel_theta_(0.0),
+      min_speed_xy_(0.0),
+      max_speed_xy_(0.0),
+      min_speed_theta_(0.0),
+      acc_lim_x_(0.0),
+      acc_lim_y_(0.0),
+      acc_lim_theta_(0.0),
+      decel_lim_x_(0.0),
+      decel_lim_y_(0.0),
+      decel_lim_theta_(0.0),
+      min_speed_xy_sq_(0.0),
+      max_speed_xy_sq_(0.0),
+      original_min_vel_x_(0.0),
+      original_min_vel_y_(0.0),
+      original_max_vel_x_(0.0),
+      original_max_vel_y_(0.0),
+      original_max_vel_theta_(0.0),
+      original_min_speed_xy_(0.0),
+      original_max_speed_xy_(0.0),
+      original_min_speed_theta_(0.0),
+      original_acc_lim_x_(0.0),
+      original_acc_lim_y_(0.0),
+      original_acc_lim_theta_(0.0),
+      original_decel_lim_x_(0.0),
+      original_decel_lim_y_(0.0),
+      original_decel_lim_theta_(0.0)
+  {
+  }
+
+  KinematicParameters::~KinematicParameters()
   {
   }
 

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -8,6 +8,16 @@
 namespace mkt_plugins
 {
 
+LimitedAccelGenerator::LimitedAccelGenerator()
+  : StandardTrajectoryGenerator(),
+    acceleration_time_(0.0)
+{
+}
+
+LimitedAccelGenerator::~LimitedAccelGenerator()
+{
+}
+
 void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
 {
   StandardTrajectoryGenerator::initialize(nh);

src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp

@@ -7,8 +7,13 @@
 namespace mkt_plugins
 {
 
-SimpleGoalChecker::SimpleGoalChecker() :
-  xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
+SimpleGoalChecker::SimpleGoalChecker()
+  : nh_(),
+    xy_goal_tolerance_(0.25),
+    yaw_goal_tolerance_(0.25),
+    stateful_(true),
+    check_xy_(true),
+    xy_goal_tolerance_sq_(0.0625)
 {
 }
 

src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp

@@ -13,6 +13,24 @@ using robot_nav_2d_utils::loadParameterWithDeprecation;
 
 namespace mkt_plugins
 {
+
+  StandardTrajectoryGenerator::StandardTrajectoryGenerator()
+    : nh_kinematics_(),
+      kinematics_(nullptr),
+      velocity_iterator_(nullptr),
+      sim_time_(0.0),
+      discretize_by_time_(false),
+      time_granularity_(0.0),
+      linear_granularity_(0.0),
+      angular_granularity_(0.0),
+      include_last_point_(false)
+  {
+  }
+
+  StandardTrajectoryGenerator::~StandardTrajectoryGenerator()
+  {
+  }
+
   void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh)
   {
     nh_kinematics_ = nh;

src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp

@@ -4,6 +4,21 @@
 
 namespace mkt_plugins
 {
+XYThetaIterator::XYThetaIterator()
+  : vx_samples_(0),
+    vy_samples_(0),
+    vtheta_samples_(0),
+    kinematics_(nullptr),
+    x_it_(nullptr),
+    y_it_(nullptr),
+    th_it_(nullptr)
+{
+}
+
+XYThetaIterator::~XYThetaIterator()
+{
+}
+
 void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) 
 {
   kinematics_ = kinematics;

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

@@ -13,7 +13,17 @@
 
 namespace two_points_planner 
 {
-  TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
+  TwoPointsPlanner::TwoPointsPlanner()
+    : initialized_(false),
+      lethal_obstacle_(0),
+      inscribed_inflated_obstacle_(0),
+      circumscribed_cost_(0),
+      allow_unknown_(false),
+      costmap_robot_(nullptr),
+      current_env_width_(0),
+      current_env_height_(0)
+  {
+  }
 
   TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
     : initialized_(false), costmap_robot_(NULL)
@@ -26,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -37,6 +45,8 @@ namespace two_points_planner
 
       name_ = name;
       costmap_robot_ = costmap_robot;
+
+      
       
       if(!costmap_robot_ || !costmap_robot_->getCostmap()) 
       {
@@ -47,6 +57,8 @@ namespace two_points_planner
       current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY();
       footprint_ = costmap_robot_->getRobotFootprint();
 
+      robot::log_info("size x: %d, size y: %d, resolution: %f", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY(), costmap_robot_->getCostmap()->getResolution());
+
       robot::log_info("TwoPointsPlanner Initialized successfully");
       initialized_ = true;
       return true;
@@ -111,6 +123,19 @@ 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);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -138,21 +163,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    unsigned int mx_start, my_start;
-    unsigned int mx_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
-      mx_start, my_start)
+    // unsigned int mx_start, my_start;
+    // unsigned int mx_end, my_end;
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    //   start.pose.position.y,
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
-        mx_end, my_end))
-    {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
-    }
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //     goal.pose.position.y,
+    //     mx_end, my_end))
+    // {
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   return false;
+    // }
     // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
@@ -165,8 +190,11 @@ namespace two_points_planner
     // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
     const double dx = goal.pose.position.x - start.pose.position.x;
     const double dy = goal.pose.position.y - start.pose.position.y;
-    double distance = std::sqrt(dx * dx + dy * dy);
+    const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
+    // Lấy độ phân giải của costmap
+    double resolution = costmap_robot_->getCostmap()->getResolution();
+    
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -177,14 +205,14 @@ namespace two_points_planner
       if(cos(goal_theta - theta) < 0) theta += M_PI;
     }
     else 
-    {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+    { 
+      robot_geometry_msgs::PoseStamped pose = start;
+      pose.pose.position.x += resolution * cos(theta);
+      pose.pose.position.y += resolution * sin(theta);
+      plan.push_back(pose);
+      plan.push_back(goal);
+      return true;
     }
-
-    // Lấy độ phân giải của costmap
-    double resolution = costmap_robot_->getCostmap()->getResolution();
-
     // Tính số điểm cần chia
     int num_points = std::ceil(distance / resolution);
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED
   src/pnkx_local_planner.cpp
   src/pnkx_go_straight_local_planner.cpp
   src/pnkx_rotate_local_planner.cpp
+  src/pnkx_docking_local_planner.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -1,8 +1,9 @@
 #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 
-#include <robot_nav_core2/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 #include <pnkx_local_planner/pnkx_local_planner.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 
 namespace pnkx_local_planner
 {
@@ -137,7 +138,7 @@ namespace pnkx_local_planner
       std::string docking_planner_name_;
       std::string docking_nav_name_;
 
-      robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
+      robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
       robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +159,7 @@ namespace pnkx_local_planner
       TFListenerPtr tf_;
       robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
-      std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
 
       robot::WallTimer detected_timeout_wt_, delayed_wt_;
@@ -169,7 +170,7 @@ namespace pnkx_local_planner
 
     bool start_docking_;
     double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
-    XmlRpc::XmlRpcValue original_papams_;
+    robot_xmlrpcpp::XmlRpcValue original_papams_;
     std::vector<DockingPlanner*> dkpl_;
 
     bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h

@@ -79,7 +79,6 @@ namespace pnkx_local_planner
      */
     robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
 
-    bool is_ready_;
   };
 
 } // namespace pnkx_local_planner

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h

@@ -52,6 +52,12 @@ namespace pnkx_local_planner
      */
     void getPlan(robot_nav_2d_msgs::Path2D &path) override;
 
+    /**
+     * @brief robot_nav_core2 getGlobalPlan - Gets the current global plan
+     * @param path The global plan
+     */
+    void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) override;
+
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *

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

@@ -14,7 +14,10 @@
 #include <boost/dll/alias.hpp>
 
 pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner()
-    : start_docking_(false)
+  : PNKXLocalPlanner(),
+    start_docking_(false),
+    original_xy_goal_tolerance_(0.0),
+    original_yaw_goal_tolerance_(0.0)
 {
 }
 
@@ -53,102 +56,106 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
-    nh_ = robot::NodeHandle("~");
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
-    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str());
-
+    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
     this->getParams(planner_nh_);
 
     std::string traj_generator_name;
-    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator"));
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(traj_generator_name);
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
-      robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name);
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
-    std::string algorithm_nav_name;
-    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    std::string algorithm_nav_name; 
+    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
     robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
-      nav_algorithm_ = algorithm_loader_();
+      nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
-      nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
-    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
+      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
-      rotate_algorithm_ = algorithm_loader_();
+      rotate_algorithm_ = rotate_algorithm_loader_();
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
-      rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
-      goal_checker_->initialize(nh_);
+      goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
+
     this->initializeOthers();
     this->getMaker();
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
@@ -159,8 +166,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
 void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
 {
   std::string maker_name, maker_sources;
-  nh_.param("maker_name", maker_name, std::string(""));
-  nh_.param("maker_sources", maker_sources, std::string(""));
+  parent_.param("maker_name", maker_name, std::string(""));
+  parent_.param("maker_sources", maker_sources, std::string(""));
+  robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
+  robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
   std::stringstream ss(maker_sources);
   std::string source;
 
@@ -168,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
   {
     if (maker_name == source)
     {
-      robot::NodeHandle source_node(nh_, source);
+      robot::NodeHandle source_node(parent_, source);
 
       if (source_node.hasParam("plugins"))
       {
-        XmlRpc::XmlRpcValue plugins;
+        robot_xmlrpcpp::XmlRpcValue plugins;
         source_node.getParam("plugins", plugins);
 
-        if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray)
+        if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
         {
           for (int i = 0; i < plugins.size(); ++i)
           {
-            if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
+            if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
             {
               std::stringstream name;
               name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
               std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
               std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
-
               // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
               DockingPlanner* dkpl = new DockingPlanner(name.str());
               dkpl->docking_planner_name_ = docking_planner_name;
@@ -212,7 +220,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
 {
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  if (!nh_.getParam(algorithm_nav_name, original_papams_))
+  if (!parent_.getParam(algorithm_nav_name, original_papams_))
     robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
   original_xy_goal_tolerance_ = xy_goal_tolerance_;
   original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
@@ -244,8 +252,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
-  nh_.setParam(algorithm_nav_name, original_papams_);
+
+  parent_.setParam(algorithm_nav_name, original_papams_);
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
   nh_algorithm.setParam("allow_rotate", false);
 }
 
@@ -276,7 +285,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -287,7 +296,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -306,15 +315,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
       std::string algorithm_nav_name;
       planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-      robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
       nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
       nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
       nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
       nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
       nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
 
-      nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -327,7 +336,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
             local_goal_pose = follow_pose;
           }
         }
-        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
         {
           throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
@@ -337,7 +346,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
@@ -357,6 +366,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::c
                                                                                                  const robot_nav_2d_msgs::Twist2D &velocity)
 {
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  robot::log_error("DEBUG 300");
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
   try
   {
@@ -385,7 +395,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
   else if (!ret_nav_)
   {
     traj = nav_algorithm_->calculator(pose, velocity);
-
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
@@ -393,9 +402,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
         traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
       }
     }
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -408,14 +425,14 @@ 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);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (start_docking_)
   {
     local_goal = goal_pose_;
@@ -434,7 +451,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     {
       robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
       robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
-      robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
     }
   }
 
@@ -459,8 +475,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
     std::string algorithm_nav_name;
     planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-    robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
-    nh_.setParam(algorithm_nav_name, original_papams_);
+    robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+    parent_.setParam(algorithm_nav_name, original_papams_);
     nh_algorithm.setParam("allow_rotate", false);
   }
   return ret_nav_ && ret_angle_ && dock_ok;
@@ -476,7 +492,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
       {
         if (!dkpl_.front()->initialized_)
         {
-          dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_);
+          dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
           dkpl_.front()->initialized_ = true;
         }
         else
@@ -488,6 +504,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             {
               if (dkpl_.front()->geLocalGoal(local_goal))
               {
+                robot::log_error("DEBUG 100");
                 dkpl_.front()->is_detected_ = true;
                 start_docking_ = true;
                 robot_nav_msgs::Path path;
@@ -509,6 +526,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             {
               if (dkpl_.front()->geLocalGoal(local_goal))
               {
+                robot::log_error("DEBUG 200");
                 dkpl_.front()->is_detected_ = true;
                 start_docking_ = true;
                 robot_nav_2d_msgs::Path2D path;
@@ -540,7 +558,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
         {
           if(!dkpl_.empty()) 
           {
-            if(dkpl_.front()) delete(dkpl_.front());
+            // if(dkpl_.front()) delete(dkpl_.front());
             dkpl_.erase(dkpl_.begin());
           }
           start_docking_ = false;
@@ -559,17 +577,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
-    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+    : initialized_(false),
+      is_detected_(false),
+      is_goal_reached_(false),
+      following_(false),
+      allow_rotate_(false),
+      xy_goal_tolerance_(0.05),
+      yaw_goal_tolerance_(0.05),
+      min_lookahead_dist_(0.4),
+      max_lookahead_dist_(1.0),
+      lookahead_time_(1.5),
+      angle_threshold_(0.4),
+      name_(name),
+      docking_planner_name_(),
+      docking_nav_name_(),
+      docking_planner_(nullptr),
+      docking_nav_(nullptr),
+      linear_(),
+      angular_(),
+      nh_(),
+      nh_priv_(),
+      tf_(nullptr),
+      costmap_robot_(nullptr),
+      traj_generator_(),
+      docking_planner_loader_(),
+      docking_nav_loader_(),
+      detected_timeout_wt_(),
+      delayed_wt_(),
+      delayed_(false),
+      detected_timeout_(false),
+      robot_base_frame_(),
+      maker_goal_frame_()
 {
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  if (docking_planner_)
+  detected_timeout_wt_.stop();
+  delayed_wt_.stop();
+  if (docking_planner_ != nullptr) {
     docking_planner_.reset();
-  if (docking_nav_)
+  }
+  if (docking_nav_ != nullptr) {
     docking_nav_.reset();
+  }
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
@@ -585,14 +636,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
+      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
         path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
       docking_planner_ = docking_planner_loader_();
       if (!docking_planner_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
       }
       docking_planner_->initialize(name_, costmap_robot_);
       robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -608,16 +660,17 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
       docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
       docking_nav_ = docking_nav_loader_();
       if (!docking_nav_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
       }
-      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_);
+      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
       docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
     }
@@ -654,13 +707,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
   nh_priv_.param("angle_threshold", angle_threshold_, 0.4);
 
-  detected_timeout_wt_ =
-      nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this);
+  robot::log_info("delay: %f", delay_time);
+  robot::log_info("timeout: %f", time_out);
+  robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
+  robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
+  robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
+  robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
+  robot::log_info("lookahead_time: %f", lookahead_time_);
+  robot::log_info("angle_threshold: %f", angle_threshold_);
+
+  detected_timeout_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
   detected_timeout_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 
-  delayed_wt_ =
-      nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this);
+  delayed_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
   delayed_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 
@@ -736,7 +798,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
   robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
   robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
   std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
-
+  robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
+  robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
     throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");

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

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

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

@@ -17,7 +17,21 @@
 #include <robot/robot.h>
 
 pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
-  : initialized_(false)
+  : initialized_(false),
+    traj_generator_(nullptr),
+    nav_algorithm_(nullptr),
+    rotate_algorithm_(nullptr),
+    goal_checker_(nullptr),
+    tf_(nullptr),
+    costmap_(nullptr),
+    costmap_robot_(nullptr),
+    info_(),
+    update_costmap_before_planning_(false),
+    ret_angle_(false),
+    ret_nav_(false),
+    yaw_goal_tolerance_(0.0),
+    xy_goal_tolerance_(0.0),
+    lock_(false)
 {
 }
 
@@ -74,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -83,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
     
     std::string algorithm_nav_name; 
@@ -93,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      // exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -121,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -129,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      // exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -145,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -153,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();
@@ -193,12 +208,14 @@ void pnkx_local_planner::PNKXLocalPlanner::reset()
 void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose)
 {
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  robot::log_error("[PNKXLocalPlanner] Receive new goal(%f, %f)!", goal_pose.pose.x, goal_pose.pose.y);
   reset();
   goal_pose_ = goal_pose;
 }
 
 void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
 {
+  robot::log_error("[PNKXLocalPlanner] size path: %d", (int)path.poses.size());
   // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
   costmap_robot_->resetLayers();
   global_plan_ = path;
@@ -220,6 +237,15 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
 
 }
 
+void pnkx_local_planner::PNKXLocalPlanner::getGlobalPlan(robot_nav_2d_msgs::Path2D &path)
+{
+  if (global_plan_.poses.empty())
+  {
+    return;
+  }
+  path = global_plan_;
+}
+
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   this->getParams(planner_nh_);

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

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