add new file nav_c_api2 and function

.gitignore

@@ -420,3 +420,5 @@ FodyWeavers.xsd
 
 build
 install
+devel
+

.gitmodules

@@ -7,9 +7,6 @@
 [submodule "src/Libraries/geometry2"]
 	path = src/Libraries/geometry2
 	url = https://git.pnkr.asia/DuongTD/geometry2.git
-[submodule "src/Libraries/xmlrpcpp"]
-	path = src/Libraries/xmlrpcpp
-	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
 [submodule "src/Libraries/laser_geometry"]
 	path = src/Libraries/laser_geometry
 	url = https://git.pnkr.asia/DuongTD/laser_geometry.git
@@ -22,13 +19,15 @@
 [submodule "src/Libraries/data_convert"]
 	path = src/Libraries/data_convert
 	url = https://git.pnkr.asia/DuongTD/data_convert.git
-
-[submodule "src/Libraries/robot_cpp"]
-	path = src/Libraries/robot_cpp
-	url = http://git.pnkx/HiepLM/robot_cpp.git
 [submodule "src/Algorithms/Packages/global_planners/custom_planner"]
 	path = src/Algorithms/Packages/global_planners/custom_planner
 	url = https://git.pnkr.asia/DuongTD/custom_planner.git
 [submodule "src/Algorithms/Packages/global_planners/dock_planner"]
 	path = src/Algorithms/Packages/global_planners/dock_planner
 	url = https://git.pnkr.asia/DuongTD/dock_planner.git
+[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

@@ -30,10 +30,6 @@ message(STATUS "========================================")
 # Build các packages theo thứ tự phụ thuộc
 # 1. Core libraries (header-only hoặc base libraries)
 
-if (NOT TARGET tf3)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3)
-endif()
-
 if (NOT TARGET robot_time)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)
 endif()
@@ -78,80 +74,170 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
-if (NOT TARGET nav_core)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
+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()
 
 if (NOT TARGET nav_grid)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) 
 endif()
 
-if (NOT TARGET nav_core2)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
+if (NOT TARGET robot_nav_core2)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core2)
 endif()
 
-if (NOT TARGET nav_core_adapter)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
+if (NOT TARGET robot_nav_core_adapter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core_adapter)
 endif()
 
 if (NOT TARGET move_base_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/move_base_core)
 endif()
 
-# if (NOT TARGET mkt_msgs)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs)
-# endif()
+if (NOT TARGET mkt_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs)
+endif()
 
-# if (NOT TARGET angles)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles)
-# endif()
+if (NOT TARGET angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles)
+endif()
 
-# if (NOT TARGET kalman)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman)
-# endif()
+if (NOT TARGET kalman)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman)
+endif()
 
-# if (NOT TARGET score_algorithm)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
-# endif()
+if (NOT TARGET score_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
+endif()
 
-# if (NOT TARGET mkt_plugins)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
-# endif()
+if (NOT TARGET mkt_plugins)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
+endif()
 
-# if (NOT TARGET mkt_algorithm)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
-# endif()
+if (NOT TARGET mkt_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
+endif()
 
-# if (NOT TARGET two_points_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
-# endif()
+if (NOT TARGET two_points_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
+endif()
 
-# if (NOT TARGET custom_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
-# endif()
+if (NOT TARGET custom_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
+endif()
 
-# if (NOT TARGET dock_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
-# endif()
+if (NOT TARGET dock_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
+endif()
 
-# if (NOT TARGET pnkx_local_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
-# endif()
+if (NOT TARGET pnkx_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
+endif()
 
+if (NOT TARGET robot_actionlib_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
+endif()
+
+if (NOT TARGET robot_move_base_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_move_base_msgs)
+endif()
+
+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)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
-# endif()
+# message(STATUS "[move_base] Shared library configured")
+if (NOT TARGET move_base)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
+endif()
 
 # C API for .NET/C# integration
-# if (NOT TARGET navigation_c_api)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
-# endif()
+if (NOT TARGET navigation_c_api)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
+endif()
 
 message(STATUS "========================================")
 message(STATUS "All packages configured successfully")
 message(STATUS "========================================")
-
-

config/config/costmap_params.yaml

@@ -0,0 +1,36 @@
+robot_costmap_2d:
+  global_frame: map
+  robot_base_frame: base_link
+  rolling_window: false
+  track_unknown_space: false
+
+  plugins:
+    - name: static_layer
+      type: StaticLayer
+
+    - name: inflation_layer
+      type: InflationLayer
+
+    - name: obstacle_layer
+      type: ObstacleLayer
+
+    - name: voxel_layer
+      type: VoxelLayer
+
+  library_path: ./libplugins.so
+  footprint:
+    - [0.3, 0.3]
+    - [0.3, -0.3]
+    - [-0.3, -0.3]
+    - [-0.3, 0.3]
+
+  transform_tolerance: 0.0
+  update_frequency: 1.0
+  width: 0.0
+  height: 0.0
+  resolution: 0.0
+  origin_x: 0.0
+  origin_y: 0.0
+
+  footprint_padding: 0.0
+  robot_radius: 0.0

config/config/inflation_layer_params.yaml

@@ -0,0 +1,5 @@
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 15.0
+  inflation_radius: 0.55

config/config/obstacle_layer_params.yaml

@@ -0,0 +1,19 @@
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  topic: "map"
+  sensor_frame: laser_frame
+  observation_persistence: 0.0
+  expected_update_rate: 0.0
+  data_type: PointCloud
+  min_obstacle_height: 0.0
+  max_obstacle_height: 2.0
+  inf_is_valid: false
+  clearing: false
+  marking: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  footprint_clearing_enabled: true
+  combination_method: 1
+

config/config/static_layer_params.yaml

@@ -0,0 +1,11 @@
+static_layer:
+  enabled: true
+  map_topic: "map"
+  first_map_only: false
+  subscribe_to_updates: false
+  track_unknown_space: true
+  use_maximum: false
+  lethal_cost_threshold: 100
+  unknown_cost_value: -1
+  trinary_costmap: true
+  base_frame_id: "map"

config/config/voxel_layer_params.yaml

@@ -0,0 +1,15 @@
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 3.0
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 16
+  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,5 +1,7 @@
-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
@@ -11,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
@@ -19,36 +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:  
+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/custom_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: CustomPlanner
 CustomPlanner:
   library_path: libcustom_planner
   environment_type: XYThetaLattice

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,17 +1,49 @@
+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: 20.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: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
-max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
+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.4
-### recovery behaviors
-recovery_behavior_enabled: true
+oscillation_distance: 0.5
+## recovery behaviors
+#
+# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
+# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
+# `recovery_behaviors` ở đây.
+#
+# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
+# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
+# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
+# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
+# config tranh nhau cùng một tên alias.
+recovery_behavior_enabled: false 
 recovery_behaviors: [       
-                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
-                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
+                      {name: aggressive_reset, type: ClearCostmapRecovery},
+                      {name: conservative_reset, type: ClearCostmapRecovery},                      
                     ]
 
 conservative_reset:
@@ -21,5 +53,8 @@ conservative_reset:
 aggressive_reset:
   reset_distance: 3.0
 
+ClearCostmapRecovery:
+  library_path: librobot_clear_costmap_recovery
+
 MoveBase:
   library_path: libmove_base

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,12 +62,12 @@ 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 (nav_msgs/Odometry,
+# The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,
 # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
-# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# robot_sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
 # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
 # default values, and must be specified.
 odom0: odom

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,16 +1,9 @@
+yaw_goal_tolerance: 0.02  
+xy_goal_tolerance: 0.03 
+min_approach_linear_velocity: 0.05 
 
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-
-base_local_planner: LocalPlannerAdapter
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017  
-  xy_goal_tolerance: 0.03 
-  min_approach_linear_velocity: 0.06   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
-
 PNKXLocalPlanner:
   # Algorithm
   library_path: libpnkx_local_planner
@@ -55,17 +48,17 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  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.9    # max_rot_vel: 1.0  # choose slightly less than the base's capability
-  min_vel_theta: 0.04  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.0
+  acc_lim_x: 3.0
   acc_lim_y: 0.0      # diff drive robot
-  acc_lim_theta: 0.9
-  decel_lim_x: -1.0
+  acc_lim_theta: 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
@@ -81,22 +74,30 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
+  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:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.6                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.5                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
   use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
@@ -105,96 +106,94 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.1
-  trans_stopped_velocity: 0.1
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
-  # Regulated linear velocity scaling
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
-  # only when true:
-  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
-  regulated_linear_scaling_min_speed: 0.15      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
-
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
-  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
-  cost_scaling_dist: 0.2                             # (default: 0.6)
-  cost_scaling_gain: 2.0                             # (default: 1.0)
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 1.2
+  final_heading_ki_angular: 0.002
+  final_heading_kd_angular: 0.12
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
-  index_samples: 60
-  follow_step_path: true
-
-  # Lookahead
-  use_velocity_scaled_lookahead_dist: true   # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
-  # only when false:
-  lookahead_dist: 0.5                        # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
-  # only when true:
-  min_lookahead_dist: 0.8                    # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 2.5                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
-
-  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
-  # only when true:
-  rotate_to_heading_min_angle: 0.35   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-
-  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
-  
-  use_regulated_linear_velocity_scaling: false
-  use_cost_regulated_linear_velocity_scaling: false
-
-MKTAlgorithmDiffRotateToGoal:
-  library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
+  xy_local_goal_tolerance: 0.05
+  angle_threshold: 0.8
   index_samples: 60
   follow_step_path: true
 
   # Lookahead
   use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
   # only when false:
-  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 0.8                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Maximum squared journey to consider for goal (default: 0.2) 
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 2.0                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 1.0                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 1.0                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
   use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
   # only when true:
   rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-  angular_decel_zone: 0.1 
-  
+  angular_decel_zone: 0.1             
+
   # stoped
   rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
+  trans_stopped_velocity: 0.03
   
-  # Regulated linear velocity scaling
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 1.5
+  final_heading_ki_angular: 0.2
+  final_heading_kd_angular: 0.05
+
+MKTAlgorithmDiffRotateToGoal:
+  library_path: libmkt_algorithm_diff
+  xy_local_goal_tolerance: 0.05
+  angle_threshold: 0.47
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
-  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
-
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
-  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
-  cost_scaling_dist: 0.2                             # (default: 0.6)
-  cost_scaling_gain: 2.0                             # (default: 1.0)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  angular_decel_zone: 0.1             
 
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
+  
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 2.0
+  near_goal_heading_kp: 1.5
+  near_goal_heading_ki: 0.0
+  near_goal_heading_kd: 0.0
+  
 GoalChecker:
   library_path: libmkt_plugins_goal_checker
 
 SimpleGoalChecker:
-  library_path: libmkt_plugins_simple_goal_checker
+  library_path: libmkt_plugins_goal_checker

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

config/two_points_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: TwoPointsPlanner
 TwoPointsPlanner:
   library_path: libtwo_points_planner
   lethal_obstacle: 20

doc/architecture_discussion.md

@@ -67,8 +67,8 @@ flowchart TB
   %% ========== COSTMAP LAYER ==========
   subgraph Costmap["🗺️ Costmap Layer"]
     direction LR
-    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
-    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
+    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
+    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
     style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
     style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
     style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
@@ -89,12 +89,12 @@ flowchart TB
   %% ========== DATA SOURCES ==========
   subgraph DataSources["📡 Data Sources"]
     direction TB
-    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
+    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
     Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
     TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
-    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
-    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
-    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
+    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
+    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 robot_sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
+    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 robot_nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
     style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
     style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
     style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
@@ -107,7 +107,7 @@ flowchart TB
   %% ========== CONTROL LOOP ==========
   subgraph ControlLoop["🔄 Control Loop"]
     direction LR
-    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
+    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 robot_geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
     BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
     style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
     style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
@@ -205,20 +205,64 @@ Cần làm rõ:
    - Thực thi control loop (executeCycle)
 
 4. **Planning Layer** ✅
-   - `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.)
-   - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
-   - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
-   - Plugin system sử dụng `boost::dll` để dynamic loading
+   - `robot_nav_core::BaseGlobalPlanner`: Interface cho global planners
+   - **Global Planner Implementations**:
+     - `custom_planner` - Custom path planner với curve support và merge path calculation
+     - `dock_planner` - Docking-specific planner với dock calculation utilities
+     - `two_points_planner` - Simple planner cho two-point navigation
+   - `robot_nav_core::BaseLocalPlanner`: Interface cho local planners
+   - **Local Planner Implementation**:
+     - `pnkx_local_planner` - Main local planner với multiple behaviors:
+       - Position planning (MKT algorithm)
+       - Docking planning
+       - Go straight planning
+       - Rotate planning
+   - `robot_nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+   - Plugin system sử dụng `boost::dll` và `robot::PluginLoaderHelper` để dynamic loading
 
 5. **Costmap Layer** ✅
-   - `costmap_2d::Costmap2DROBOT`: Global và local costmap
-   - Costmap layers: static map, obstacles, inflation
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap wrapper
+   - `robot_costmap_2d::LayeredCostmap`: Layered costmap system
+   - **Costmap Layers** (8 implementations):
+     - `StaticLayer` - Static map từ OccupancyGrid
+     - `ObstacleLayer` - Dynamic obstacles từ sensors
+     - `InflationLayer` - Cost inflation cho robot footprint
+     - `VoxelLayer` - 3D voxel grid cho obstacles
+     - `CriticalLayer` - Critical zones marking
+     - `DirectionalLayer` - Directional cost preferences
+     - `PreferredLayer` - Preferred path zones
+     - `UnpreferredLayer` - Unpreferred zones
    - Frame management: map (global), odom (local)
+   - Thread-safe access với mutex
+   - Observation buffer cho sensor data
 
 6. **Algorithms Layer** ✅
-   - `mkt_algorithm`: Diff drive và bicycle kinematics algorithms
-   - `score_algorithm`: Trajectory scoring và goal checking
-   - `kalman`: Filtering algorithms
+   - **MKT Algorithm** (`mkt_algorithm`):
+     - Differential drive kinematics:
+       - `diff_go_straight` - Go straight behavior
+       - `diff_predictive_trajectory` - Predictive trajectory following
+       - `diff_rotate_to_goal` - Rotate to goal
+     - Bicycle model kinematics:
+       - `bicycle_go_straight` - Go straight với bicycle model
+       - `bicycle_pure_pursuit` - Pure pursuit control
+       - `bicycle_rotate_to_goal` - Rotate behavior
+   - **MKT Plugins** (`mkt_plugins`):
+     - `goal_checker` - Goal checking algorithms
+     - `simple_goal_checker` - Simple goal checking
+     - `standard_traj_generator` - Standard trajectory generation
+     - `limited_accel_generator` - Acceleration-limited trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration utilities
+     - `velocity_iterator` - Velocity space iteration
+     - `one_d_velocity_iterator` - 1D velocity iteration
+   - **Score Algorithm** (`score_algorithm`):
+     - Trajectory scoring và evaluation
+     - Goal checking utilities
+     - Trajectory generator interface
+   - **Kalman Filter** (`kalman`):
+     - State estimation
+     - Sensor fusion
+     - Noise filtering
 
 7. **Data Sources** ⚠️ (Interface cần định nghĩa)
    - Localization source (Pnkx Loc)
@@ -232,10 +276,10 @@ Cần làm rõ:
    - Velocity command execution
 
 **Định dạng dữ liệu:**
-- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
-- `geometry_msgs::Twist` (vận tốc linear/angular)
-- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
-- `costmap_2d::Costmap2D` (bản đồ chi phí)
+- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
+- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
+- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
+- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)
 
 ### 3. Thiết kế từng module (interface level)
 
@@ -250,21 +294,25 @@ Cần làm rõ:
   - `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái
   - `getRobotPose(...)` - Lấy vị trí robot
 
-- `nav_core::BaseGlobalPlanner` ✅
+- `robot_nav_core::BaseGlobalPlanner` ✅
   - `makePlan(start, goal, plan)` - Tạo global path
   - `initialize(name, costmap_robot)` - Khởi tạo với costmap
+  - **Implementations**: `custom_planner`, `dock_planner`, `two_points_planner`
 
-- `nav_core::BaseLocalPlanner` ✅
-  - `computeVelocityCommands(cmd_vel)` - Tính toán velocity command
+- `robot_nav_core::BaseLocalPlanner` ✅
+  - `computeVelocityCommands(odom, cmd_vel)` - Tính toán velocity command
   - `setPlan(plan)` - Set global path để follow
   - `isGoalReached()` - Kiểm tra đã đến goal chưa
-  - `swapPlanner(name)` - Thay đổi planner động
+  - `swapPlanner(name)` - Thay đổi planner động (position, docking, go_straight, rotate)
   - `setTwistLinear/Angular(...)` - Set velocity limits
+  - `getTwistLinear/Angular(...)` - Get velocity limits
+  - **Implementation**: `pnkx_local_planner` với multiple behaviors
 
-- `nav_core::RecoveryBehavior` ✅
+- `robot_nav_core::RecoveryBehavior` ✅
   - `runBehavior()` - Thực thi recovery behavior
+  - `initialize(name, tf, global_costmap, local_costmap)` - Khởi tạo
 
-- `costmap_2d::Costmap2DROBOT` ✅
+- `robot_costmap_2d::Costmap2DROBOT` ✅
   - Wrapper cho costmap với robot footprint
   - Thread-safe access với mutex
 
@@ -290,9 +338,15 @@ Cần làm rõ:
 
 **Plugin mechanism:** ✅
 - Sử dụng `boost::dll` để dynamic loading plugins
-- Factory pattern với `boost::function` và `boost::dll::import`
+- Factory pattern với `boost::function` và `boost::dll::import_alias`
+- `robot::PluginLoaderHelper` để tìm library path từ plugin name
 - Config file YAML để specify plugin names
-- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior`
+- Plugin interfaces: 
+  - `robot_nav_core::BaseGlobalPlanner` - Global planners
+  - `robot_nav_core::BaseLocalPlanner` - Local planners  
+  - `robot_nav_core::RecoveryBehavior` - Recovery behaviors
+  - `robot_costmap_2d::Layer` - Costmap layers
+- Plugin registration với `BOOST_DLL_ALIAS` macro
 
 ### 4. Cơ chế giao tiếp & đồng bộ
 
@@ -340,36 +394,99 @@ Cần làm rõ:
 
 **Đã hoàn thành:** ✅
 
-1. ✅ **Interface Layer**: `BaseNavigation` interface
-2. ✅ **Implementation Layer**: `MoveBase` core logic
-3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
-4. ✅ **Costmap Layer**: Global và local costmap với layers
-5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
-6. ✅ **API Layer**: C API wrapper cho .NET integration
-7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, robot_nav_2d_utils
+1. ✅ **Interface Layer**: 
+   - `move_base_core::BaseNavigation` - Abstract interface hoàn chỉnh
+   - `move_base_core::common.h` - Common types và utilities
+
+2. ✅ **Implementation Layer**: 
+   - `move_base::MoveBase` - Core implementation với state machine
+   - `executeCycle()` - Control loop implementation
+   - State management (PLANNING, CONTROLLING, CLEARING, PAUSED)
+   - Threading với planner thread và control loop
+
+3. ✅ **Planning Layer - Plugin System**: 
+   - **Global Planners** (3 implementations):
+     - `custom_planner` - Custom path planning với curve support
+     - `dock_planner` - Docking-specific planner
+     - `two_points_planner` - Simple two-point planner
+   - **Local Planners** (1 implementation):
+     - `pnkx_local_planner` - Main local planner với:
+       - `pnkx_docking_local_planner` - Docking behavior
+       - `pnkx_go_straight_local_planner` - Go straight behavior
+       - `pnkx_rotate_local_planner` - Rotate behavior
+   - **Recovery Behaviors**: Interface đã định nghĩa
+   - Plugin loading với `boost::dll` và `robot::PluginLoaderHelper`
+
+4. ✅ **Costmap Layer**: 
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - **Costmap Layers** (8 implementations):
+     - `static_layer` - Static map layer
+     - `obstacle_layer` - Dynamic obstacles
+     - `inflation_layer` - Cost inflation
+     - `voxel_layer` - 3D voxel grid
+     - `critical_layer` - Critical zones
+     - `directional_layer` - Directional costs
+     - `preferred_layer` - Preferred paths
+     - `unpreferred_layer` - Unpreferred zones
+   - Layered costmap system với plugin support
+
+5. ✅ **Algorithms Layer**: 
+   - **MKT Algorithm**:
+     - Differential drive kinematics (go_straight, rotate, predictive)
+     - Bicycle model kinematics (go_straight, rotate, pure pursuit)
+   - **MKT Plugins**:
+     - `goal_checker` - Goal checking algorithms
+     - `standard_traj_generator` - Standard trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration
+     - `limited_accel_generator` - Acceleration-limited trajectories
+   - **Score Algorithm**: Trajectory scoring và goal checking
+   - **Kalman Filter**: State estimation và filtering
+
+6. ✅ **API Layer**: 
+   - `nav_c_api` - C API wrapper cho .NET/C# P/Invoke
+   - Wrapper functions cho BaseNavigation methods
+
+7. ✅ **Supporting Libraries**: 
+   - `tf3` - Transform system (buffer_core, transforms)
+   - `robot_time` - Time management (Time, Duration, Timer, Rate)
+   - `robot_cpp` - Core utilities:
+     - `robot::init()` - Initialization
+     - `robot::NodeHandle` - YAML config loading
+     - `robot::PluginLoaderHelper` - Plugin discovery
+   - `robot_nav_2d_utils` - 2D navigation utilities (conversions, path_ops, polygons, bounds, tf_help)
+   - `robot_nav_2d_msgs` - 2D navigation messages
+   - `geometry_msgs` - Geometry message types (29 types)
+   - `robot_nav_msgs` - Navigation messages (24 types)
+   - `robot_sensor_msgs` - Sensor messages (30 types)
+   - `laser_geometry` - Laser scan geometry utilities
+   - `voxel_grid` - Voxel grid implementation
+   - `data_convert` - Data conversion utilities
+   - `nav_grid` - Navigation grid utilities
 
 **Đang triển khai / Cần bổ sung:** ⚠️
 
 1. ⚠️ **Data Sources Interfaces**: 
-   - `ILocalizationSource` interface
-   - `IOdometrySource` interface
-   - `IMapProvider` interface
-   - Integration với Pnkx Loc, odometry sources
+   - `ILocalizationSource` interface - Cần định nghĩa
+   - `IOdometrySource` interface - Cần định nghĩa
+   - `IMapProvider` interface - Cần định nghĩa
+   - Integration với Pnkx Loc, odometry sources - Hiện tại dùng trực tiếp qua MoveBase methods
 
 2. ⚠️ **Base Controller**:
-   - `IBaseController` interface
-   - Diff drive controller implementation
-   - Steer drive controller implementation
-   - Velocity command execution
+   - `IBaseController` interface - Cần định nghĩa
+   - Diff drive controller implementation - Cần implement
+   - Steer drive controller implementation - Cần implement
+   - Velocity command execution - Hiện tại chỉ có `setTwistLinear/Angular` trong local planner
 
-3. ⚠️ **Control Loop**:
-   - Control loop trong MoveBase (executeCycle)
-   - State machine management hoàn chỉnh
-   - Threading và synchronization
+3. ⚠️ **User Controller Plugin System**:
+   - Factory để load user controller plugins - Cần implement
+   - Interface cho User Controller - Cần định nghĩa
+   - Integration với BaseNavigation - Cần implement
 
-4. ⚠️ **User Controller Plugin System**:
-   - Factory để load user controller plugins
-   - Integration với BaseNavigation
+4. ⚠️ **Testing & Documentation**:
+   - Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
+   - Integration tests - Cần bổ sung
+   - Simulation environment - TODO
 
 **Lộ trình tiếp theo:**
 
@@ -406,38 +523,144 @@ pnkx_nav_core/
 ├── src/
 │   ├── Navigations/
 │   │   ├── Cores/
-│   │   │   ├── move_base_core/     # BaseNavigation interface
-│   │   │   ├── nav_core/            # Planner interfaces
-│   │   │   ├── nav_core_adapter/    # Adapter utilities
-│   │   │   └── nav_core2/           # Additional nav utilities
+│   │   │   ├── move_base_core/              # BaseNavigation interface ✅
+│   │   │   │   ├── include/move_base_core/
+│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
+│   │   │   │   │   └── common.h             # Common types (TFListenerPtr, etc.)
+│   │   │   ├── robot_nav_core/              # Planner interfaces ✅
+│   │   │   │   ├── include/robot_nav_core/
+│   │   │   │   │   ├── base_global_planner.h
+│   │   │   │   │   ├── base_local_planner.h
+│   │   │   │   │   └── recovery_behavior.h
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities ✅
+│   │   │   │   ├── include/robot_nav_core_adapter/
+│   │   │   │   │   ├── global_planner_adapter.h
+│   │   │   │   │   ├── local_planner_adapter.h
+│   │   │   │   │   └── costmap_adapter.h
+│   │   │   └── robot_nav_core2/             # Additional nav utilities ✅
+│   │   │       ├── include/robot_nav_core2/
+│   │   │       │   ├── global_planner.h
+│   │   │       │   ├── local_planner.h
+│   │   │       │   └── costmap.h
 │   │   ├── Libraries/
-│   │   │   ├── costmap_2d/          # Costmap system
-│   │   │   ├── tf3/                 # Transform system
-│   │   │   ├── robot_time/          # Time management
-│   │   │   ├── geometry2/           # Geometry utilities
-│   │   │   └── ...                  # Other supporting libraries
+│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
+│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
 │   │   └── Packages/
-│   │       └── move_base/           # MoveBase implementation
+│   │       └── move_base/                   # MoveBase implementation ✅
+│   │           ├── include/move_base/
+│   │           │   └── move_base.h
+│   │           └── src/
+│   │               ├── move_base.cpp
+│   │               └── move_base_main.cpp
+│   │
 │   ├── Algorithms/
 │   │   ├── Cores/
-│   │   │   └── score_algorithm/     # Trajectory scoring
-│   │   └── Libraries/
-│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
-│   │       ├── kalman/              # Kalman filtering
-│   │       └── angles/               # Angle utilities
+│   │   │   └── score_algorithm/             # Trajectory scoring ✅
+│   │   │       ├── include/score_algorithm/
+│   │   │       │   ├── score_algorithm.h
+│   │   │       │   ├── goal_checker.h
+│   │   │       │   └── trajectory_generator.h
+│   │   ├── Libraries/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics algorithms ✅
+│   │   │   │   ├── include/mkt_algorithm/
+│   │   │   │   │   ├── diff/                # Differential drive
+│   │   │   │   │   │   ├── diff_go_straight.h
+│   │   │   │   │   │   ├── diff_predictive_trajectory.h
+│   │   │   │   │   │   └── diff_rotate_to_goal.h
+│   │   │   │   │   └── bicycle/             # Bicycle model
+│   │   │   │   │       ├── bicycle.h
+│   │   │   │   │       ├── go_straight.h
+│   │   │   │   │       └── rotate_to_goal.h
+│   │   │   ├── mkt_plugins/                 # MKT plugin components ✅
+│   │   │   │   ├── include/mkt_plugins/
+│   │   │   │   │   ├── goal_checker.h
+│   │   │   │   │   ├── standard_traj_generator.h
+│   │   │   │   │   ├── kinematic_parameters.h
+│   │   │   │   │   └── xy_theta_iterator.h
+│   │   │   ├── mkt_msgs/                    # MKT message types ✅
+│   │   │   ├── kalman/                      # Kalman filtering ✅
+│   │   │   └── angles/                      # Angle utilities ✅
+│   │   └── Packages/
+│   │       ├── global_planners/
+│   │       │   ├── custom_planner/           # Custom global planner ✅
+│   │       │   ├── dock_planner/            # Docking planner ✅
+│   │       │   └── two_points_planner/      # Two points planner ✅
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner ✅
+│   │               ├── include/pnkx_local_planner/
+│   │               │   ├── pnkx_local_planner.h
+│   │               │   ├── pnkx_docking_local_planner.h
+│   │               │   ├── pnkx_go_straight_local_planner.h
+│   │               │   └── pnkx_rotate_local_planner.h
+│   │
+│   ├── Libraries/
+│   │   ├── costmap_2d/                      # Costmap system ✅
+│   │   │   ├── include/robot_costmap_2d/
+│   │   │   │   ├── costmap_2d_robot.h
+│   │   │   │   ├── layered_costmap.h
+│   │   │   │   └── layer.h
+│   │   │   ├── plugins/                     # Costmap layers ✅
+│   │   │   │   ├── static_layer.cpp
+│   │   │   │   ├── obstacle_layer.cpp
+│   │   │   │   ├── inflation_layer.cpp
+│   │   │   │   ├── voxel_layer.cpp
+│   │   │   │   ├── critical_layer.cpp
+│   │   │   │   ├── directional_layer.cpp
+│   │   │   │   ├── preferred_layer.cpp
+│   │   │   │   └── unpreferred_layer.cpp
+│   │   │   └── config/                      # Costmap configs
+│   │   ├── tf3/                             # Transform system ✅
+│   │   ├── robot_time/                      # Time management ✅
+│   │   ├── robot_cpp/                       # Core utilities ✅
+│   │   │   ├── include/robot/
+│   │   │   │   ├── init.h                   # Initialization
+│   │   │   │   ├── node_handle.h            # NodeHandle (YAML config)
+│   │   │   │   └── plugin_loader_helper.h   # Plugin loader
+│   │   ├── robot_nav_2d_utils/              # 2D nav utilities ✅
+│   │   ├── robot_nav_2d_msgs/               # 2D nav messages ✅
+│   │   ├── geometry2/                       # Geometry utilities ✅
+│   │   │   ├── robot_tf3_geometry_msgs/
+│   │   │   └── robot_tf3_sensor_msgs/
+│   │   ├── common_msgs/                     # Message types ✅
+│   │   │   ├── robot_geometry_msgs/
+│   │   │   ├── robot_nav_msgs/
+│   │   │   ├── robot_sensor_msgs/
+│   │   │   ├── robot_std_msgs/
+│   │   │   ├── robot_protocol_msgs/
+│   │   │   └── robot_visualization_msgs/
+│   │   ├── laser_geometry/                  # Laser geometry ✅
+│   │   ├── voxel_grid/                      # Voxel grid ✅
+│   │   ├── data_convert/                    # Data conversion ✅
+│   │   └── xmlrpcpp/                        # XML-RPC utilities ✅
+│   │
 │   └── APIs/
-│       └── c_api/                   # C API wrapper
-├── build/                            # Build artifacts
-└── doc/                              # Documentation
+│       └── c_api/                           # C API wrapper ✅
+│           ├── include/nav_c_api.h
+│           └── src/nav_c_api.cpp
+│
+├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
+└── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    └── folders.md
 ```
 
 ## Ghi chú
 
 - ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
+- ✅ **3 Global Planners** đã triển khai: custom_planner, dock_planner, two_points_planner
+- ✅ **1 Local Planner** đã triển khai: pnkx_local_planner với 4 behaviors (position, docking, go_straight, rotate)
+- ✅ **8 Costmap Layers** đã triển khai: static, obstacle, inflation, voxel, critical, directional, preferred, unpreferred
+- ✅ **MKT Algorithms** hỗ trợ cả differential drive và bicycle model
+- ✅ **Plugin System** hoàn chỉnh với `boost::dll` và `robot::PluginLoaderHelper`
 - ⚠️ Cần bổ sung data sources interfaces và base controller
 - 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system
-- 📦 Tất cả components được build bằng CMake, không phụ thuộc ROS
+- 📦 Tất cả components được build bằng CMake, hỗ trợ cả Catkin và Standalone CMake
 - 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading
+- 🎯 MoveBase đã có control loop (`executeCycle`) và state machine hoàn chỉnh
+- 📚 Có đầy đủ message types: geometry_msgs (29), nav_msgs (24), sensor_msgs (30), std_msgs (32)
 
 
 

doc/folders.md

@@ -1,53 +1,99 @@
-Mô tả cấu trúc:
+# Cấu trúc thư mục pnkx_nav_core
 
-├── common_msgs/
-│   ├── build/
-│   ├── geometry_msgs/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── nav_msgs/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── sensor_msgs/
-│   │   ├── cfg/
-│   │   ├── include/
-│   │   └── test/
-│   ├── CMakeLists.txt
-│   ├── std_msgs/
-│   │   ├── include/
-│   │   └── CMakeLists.txt
-│   └── CMakeLists.txt (root)
-|
-├── Navigations/
-│   ├── Cores/
-│   │   └── move_base_core/
-│   │       ├── build/
-│   │       ├── example/
-│   │       ├── include/
-│   │       ├── .gitignore
-│   │       ├── CMakeLists.txt
-│   │       └── README.md
+Mô tả cấu trúc thư mục thực tế của dự án:
+
+```
+pnkx_nav_core/
+├── src/
+│   ├── Navigations/
+│   │   ├── Cores/
+│   │   │   ├── move_base_core/              # BaseNavigation interface
+│   │   │   ├── robot_nav_core/              # Planner interfaces (BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior)
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities (global_planner_adapter, local_planner_adapter, costmap_adapter)
+│   │   │   └── robot_nav_core2/             # Additional nav utilities (global_planner, local_planner, costmap)
+│   │   ├── Libraries/
+│   │   │   └── nav_grid/                    # Navigation grid utilities
+│   │   └── Packages/
+│   │       └── move_base/                   # MoveBase implementation
+│   │
+│   ├── Algorithms/
+│   │   ├── Cores/
+│   │   │   └── score_algorithm/             # Trajectory scoring và goal checking
+│   │   ├── Libraries/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics (diff drive & bicycle)
+│   │   │   ├── mkt_plugins/                 # MKT plugin components
+│   │   │   ├── mkt_msgs/                    # MKT message types
+│   │   │   ├── kalman/                      # Kalman filtering
+│   │   │   └── angles/                      # Angle utilities
+│   │   └── Packages/
+│   │       ├── global_planners/
+│   │       │   ├── custom_planner/           # Custom global planner
+│   │       │   ├── dock_planner/            # Docking planner
+│   │       │   └── two_points_planner/      # Two points planner
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner
+│   │
 │   ├── Libraries/
-│   │   ├── geometry_msgs/
-│   │   │   ├── build/
-│   │   │   ├── include/
-│   │   │   └── CMakeLists.txt
-│   │   └── libtf2/
-│   │       ├── .vscode/
-│   │       ├── include/
-│   │       ├── samples/
-│   │       ├── src/
-│   │       ├── .gitignore
-│   │       ├── DEBIAN_PACKAGING.md
-│   │       ├── libtf2_2.0.0_amd64.deb
-│   │       ├── Makefile
-│   │       └── README.md
-│   ├── Packages/
-│   │   └── move_base/
-│   │       ├── build/
-│   │       ├── include/
-│   │       ├── src/
-│   │       └── CMakeLists.txt
-│   └── CMakeLists.txt (root)
+│   │   ├── costmap_2d/                      # Costmap system với 8 layers
+│   │   ├── tf3/                             # Transform system
+│   │   ├── robot_time/                      # Time management
+│   │   ├── robot_cpp/                       # Core utilities (init, NodeHandle, PluginLoaderHelper)
+│   │   ├── robot_nav_2d_utils/             # 2D navigation utilities
+│   │   ├── robot_nav_2d_msgs/              # 2D navigation messages
+│   │   ├── geometry2/                       # Geometry utilities (tf3_geometry_msgs, tf3_sensor_msgs)
+│   │   ├── common_msgs/                     # Message types
+│   │   │   ├── robot_geometry_msgs/         # Geometry messages (29 types)
+│   │   │   ├── robot_nav_msgs/             # Navigation messages (24 types)
+│   │   │   ├── robot_sensor_msgs/          # Sensor messages (30 types)
+│   │   │   ├── robot_std_msgs/             # Standard messages (32 types)
+│   │   │   ├── robot_protocol_msgs/        # Protocol messages
+│   │   │   ├── robot_visualization_msgs/   # Visualization messages
+│   │   │   └── utils/                       # Message utilities
+│   │   ├── laser_geometry/                  # Laser geometry utilities
+│   │   ├── voxel_grid/                      # Voxel grid
+│   │   ├── data_convert/                    # Data conversion
+│   │   └── xmlrpcpp/                        # XML-RPC utilities
+│   │
+│   └── APIs/
+│       └── c_api/                           # C API wrapper cho .NET/C#
+│
+├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
+└── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    ├── folders.md
+    └── readme.md
+```
+
+## Chi tiết các thư mục chính
+
+### Navigations/
+- **Cores/**: Interfaces và core utilities
+  - `move_base_core`: BaseNavigation interface
+  - `robot_nav_core`: Planner interfaces
+  - `robot_nav_core_adapter`: Adapter layer
+  - `robot_nav_core2`: Additional utilities
+- **Libraries/**: Navigation libraries
+  - `nav_grid`: Grid utilities
+- **Packages/**: Implementations
+  - `move_base`: MoveBase implementation
+
+### Algorithms/
+- **Cores/**: Core algorithms
+  - `score_algorithm`: Trajectory scoring
+- **Libraries/**: Algorithm libraries
+  - `mkt_algorithm`: Kinematics algorithms
+  - `mkt_plugins`: Plugin components
+  - `kalman`: Filtering
+- **Packages/**: Algorithm implementations
+  - `global_planners/`: 3 planners
+  - `local_planners/`: 1 planner với multiple behaviors
+
+### Libraries/
+- **costmap_2d/**: Costmap system với 8 layers
+- **tf3/**: Transform system
+- **robot_cpp/**: Core utilities (init, NodeHandle, PluginLoaderHelper)
+- **common_msgs/**: Message type definitions
+- Các utilities khác: time, geometry, sensors, etc.

doc/implementation_plan.md

@@ -15,7 +15,7 @@
    - Plugin system với `boost::dll` để load dynamic plugins
 
 3. **Costmap System:**
-   - `costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
    - Costmap layers (static, obstacle, inflation, etc.)
 
 4. **Supporting Libraries:**
@@ -56,8 +56,8 @@
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
 ```cpp
 class IOdometrySource {
-  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
   virtual bool isAvailable() = 0;
 };
 ```
@@ -66,7 +66,7 @@ class IOdometrySource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
 ```cpp
 class ILocalizationSource {
-  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
+  virtual robot_geometry_msgs::PoseStamped getRobotPose() = 0;
   virtual std::string getGlobalFrame() = 0;
   virtual bool isLocalized() = 0;
 };
@@ -76,7 +76,7 @@ class ILocalizationSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h`
 ```cpp
 class IMapProvider {
-  virtual nav_msgs::OccupancyGrid getMap() = 0;
+  virtual robot_nav_msgs::OccupancyGrid getMap() = 0;
   virtual bool isMapAvailable() = 0;
 };
 ```
@@ -85,7 +85,7 @@ class IMapProvider {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h`
 ```cpp
 class ISensorSource {
-  virtual sensor_msgs::LaserScan getLatestScan() = 0;
+  virtual robot_sensor_msgs::LaserScan getLatestScan() = 0;
   virtual bool hasNewData() = 0;
 };
 ```
@@ -96,9 +96,9 @@ class ISensorSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
 ```cpp
 class IBaseController {
-  virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
+  virtual bool setVelocity(const robot_geometry_msgs::Twist& cmd_vel) = 0;
   virtual bool stop() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
 };
 ```
 
@@ -120,7 +120,7 @@ class IBaseController {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
 ```cpp
 class IUserController {
-  virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
+  virtual void onGoalReceived(const robot_geometry_msgs::PoseStamped& goal) = 0;
   virtual void onNavigationStateChanged(move_base_core::State state) = 0;
   virtual void onFeedback(const NavFeedback& feedback) = 0;
 };

examples/CSharpExample.cs

@@ -1,360 +0,0 @@
-using System;
-using System.Runtime.InteropServices;
-using System.Runtime.CompilerServices;
-
-namespace NavigationExample
-{
-    /// <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 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 Header
-        {
-            public uint seq;
-            public long 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 Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str; // char*
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        // ============================================================================
-        // String Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        // ============================================================================
-        // State Conversion
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        // ============================================================================
-        // Helper Functions
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_2d(
-            double pose_x, double pose_y, double pose_theta,
-            string frame_id, double offset_distance,
-            ref PoseStamped out_goal);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_stamped(
-            ref PoseStamped in_pose, double offset_distance,
-            ref PoseStamped out_goal);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(IntPtr handle);
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool tf_listener_set_static_transform(
-            IntPtr tf_handle,
-            string parent_frame,
-            string child_frame,
-            double x, double y, double z,
-            double qx, double qy, double qz, double qw);
-
-        // ============================================================================
-        // Navigation Interface Methods
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(
-            IntPtr handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(
-            IntPtr handle, ref 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(
-            IntPtr handle, string marker, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(
-            IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(
-            IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_pause(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_resume(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_cancel(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(
-            IntPtr handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(
-            IntPtr handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(
-            IntPtr handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(
-            IntPtr handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(
-            IntPtr handle, ref NavFeedback out_feedback);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_feedback(ref NavFeedback feedback);
-
-        // ============================================================================
-        // Helper Methods for String Conversion
-        // ============================================================================
-        public static string MarshalString(IntPtr ptr)
-        {
-            if (ptr == IntPtr.Zero)
-                return string.Empty;
-            return Marshal.PtrToStringAnsi(ptr);
-        }
-    }
-
-    // ============================================================================
-    // Example Usage
-    // ============================================================================
-    class Program
-    {
-        // Helper method để hiển thị file và line number tự động
-        static void LogError(string message,
-            [CallerFilePath] string filePath = "",
-            [CallerLineNumber] int lineNumber = 0,
-            [CallerMemberName] string memberName = "")
-        {
-            // Lấy tên file từ đường dẫn đầy đủ
-            string fileName = System.IO.Path.GetFileName(filePath);
-            Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
-        }
-
-        static void Main(string[] args)
-        {
-            // Create TF listener
-            IntPtr tfHandle = NavigationAPI.tf_listener_create();
-            if (tfHandle == IntPtr.Zero)
-            {
-                LogError("Failed to create TF listener");
-                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
-            IntPtr navHandle = NavigationAPI.navigation_create();
-            if (navHandle == IntPtr.Zero)
-            {
-                LogError("Failed to create navigation instance");
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            // Initialize navigation
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
-            {
-                LogError("Failed to initialize navigation");
-                NavigationAPI.navigation_destroy(navHandle);
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            // Set robot footprint
-            NavigationAPI.Point[] footprint = new NavigationAPI.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 }
-            };
-            NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
-
-            // Get robot pose
-            NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
-            if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
-            {
-                Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
-            }
-
-            // Get navigation feedback
-            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
-            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
-            {
-                string stateStr = NavigationAPI.MarshalString(
-                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
-                NavigationAPI.navigation_free_feedback(ref feedback);
-            }
-
-            // Cleanup
-            NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
-        }
-    }
-}
-

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,10 +1,12 @@
 <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.7" />
+  </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>

examples/NavigationExample/Program.cs

@@ -1,253 +1,25 @@
 using System;
+using System.Globalization;
+using System.IO;
 using System.Runtime.InteropServices;
 using System.Runtime.CompilerServices;
+using System.Text.RegularExpressions;
+using SixLabors.ImageSharp;
+using SixLabors.ImageSharp.PixelFormats;
+using NavigationExample;
+
 
 namespace NavigationExample
 {
-    /// <summary>
-    /// C# P/Invoke wrapper for Navigation C API
-    /// </summary>
-    public class NavigationAPI
+    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
+    public struct MazeMapConfig
     {
-        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 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 Header
-        {
-            public uint seq;
-            public long 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 Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str; // char*
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        // ============================================================================
-        // String Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        // ============================================================================
-        // State Conversion
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        // ============================================================================
-        // Helper Functions
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_2d(
-            double pose_x, double pose_y, double pose_theta,
-            string frame_id, double offset_distance,
-            ref PoseStamped out_goal);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_stamped(
-            ref PoseStamped in_pose, double offset_distance,
-            ref PoseStamped out_goal);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(IntPtr handle);
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool tf_listener_set_static_transform(
-            IntPtr tf_handle,
-            string parent_frame,
-            string child_frame,
-            double x, double y, double z,
-            double qx, double qy, double qz, double qw);
-
-        // ============================================================================
-        // Navigation Interface Methods
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(
-            IntPtr handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(
-            IntPtr handle, ref 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(
-            IntPtr handle, string marker, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(
-            IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(
-            IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_pause(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_resume(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_cancel(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(
-            IntPtr handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(
-            IntPtr handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(
-            IntPtr handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(
-            IntPtr handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(
-            IntPtr handle, ref NavFeedback out_feedback);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_feedback(ref NavFeedback feedback);
-
-        // ============================================================================
-        // Helper Methods for String Conversion
-        // ============================================================================
-        public static string MarshalString(IntPtr ptr)
-        {
-            if (ptr == IntPtr.Zero)
-                return string.Empty;
-            return Marshal.PtrToStringAnsi(ptr);
-        }
+        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
+        public float Resolution;
+        public double OriginX, OriginY, OriginZ;
+        public int Negate;            // 0 hoặc 1
+        public double OccupiedThresh;
+        public double FreeThresh;
     }
 
     // ============================================================================
@@ -255,6 +27,105 @@ namespace NavigationExample
     // ============================================================================
     class Program
     {
+        /// <summary>
+        /// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
+        /// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
+        /// </summary>
+        static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
+        {
+            config = default;
+            if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
+                return false;
+            string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
+            try
+            {
+                string[] lines = File.ReadAllLines(yamlPath);
+                foreach (string line in lines)
+                {
+                    string trimmed = line.Trim();
+                    if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
+                    int colon = trimmed.IndexOf(':');
+                    if (colon <= 0) continue;
+                    string key = trimmed.Substring(0, colon).Trim();
+                    string value = trimmed.Substring(colon + 1).Trim();
+                    if (key == "image")
+                        config.ImagePath = Path.Combine(dir, value);
+                    else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
+                        config.Resolution = res;
+                    else if (key == "origin")
+                    {
+                        var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
+                        if (m.Success)
+                        {
+                            config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
+                            config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
+                            config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
+                        }
+                    }
+                    else if (key == "negate" && int.TryParse(value, out int neg))
+                        config.Negate = neg;
+                    else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
+                        config.OccupiedThresh = ot;
+                    else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
+                        config.FreeThresh = ft;
+                }
+                return !string.IsNullOrEmpty(config.ImagePath);
+            }
+            catch
+            {
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
+        /// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
+        /// </summary>
+        static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
+            int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
+        {
+            width = 0;
+            height = 0;
+            if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
+            {
+                LogError($"File not found: {imagePath}");
+                return null;
+            }
+            try
+            {
+                using var image = Image.Load<Rgba32>(imagePath);
+                int w = image.Width;
+                int h = image.Height;
+                width = w;
+                height = h;
+                byte[] data = new byte[w * h];
+                image.ProcessPixelRows(accessor =>
+                {
+                    for (int y = 0; y < accessor.Height; y++)
+                    {
+                        Span<Rgba32> row = accessor.GetRowSpan(y);
+                        for (int x = 0; x < row.Length; x++)
+                        {
+                            ref readonly Rgba32 p = ref row[x];
+                            double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
+                            if (negate != 0) gray = 1.0 - gray;
+                            byte occ;
+                            if (gray <= freeThresh) occ = 0;
+                            else if (gray >= occupiedThresh) occ = 100;
+                            else occ = 255; // unknown
+                            data[y * w + x] = occ;
+                        }
+                    }
+                });
+                return data;
+            }
+            catch (Exception ex)
+            {
+                LogError($"Load image failed: {ex.Message}");
+                return null;
+            }
+        }
+
         // Helper method để hiển thị file và line number tự động
         static void LogError(string message,
             [CallerFilePath] string filePath = "",
@@ -268,93 +139,359 @@ namespace NavigationExample
 
         static void Main(string[] args)
         {
-            // Create TF listener
-            IntPtr tfHandle = NavigationAPI.tf_listener_create();
-            if (tfHandle == 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
-            IntPtr navHandle = NavigationAPI.navigation_create();
-            if (navHandle == IntPtr.Zero)
+            // 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 robot pose
-            NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
-            if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
+  
+            IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
+            Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
+            LaserScan fscanHandle;
+            fscanHandle.header = fscanHeader;
+            fscanHandle.angle_min = -1.57f;
+            fscanHandle.angle_max = 1.57f;
+            fscanHandle.angle_increment = 0.785f;
+            fscanHandle.time_increment = 0.0f;
+            fscanHandle.scan_time = 0.1f;
+            fscanHandle.range_min = 0.05f;
+            fscanHandle.range_max = 10.0f;
+            fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5);
+            fscanHandle.ranges_count = new UIntPtr(5);
+            fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5);
+            fscanHandle.intensities_count = new UIntPtr(5);
+            NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);
+
+            IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
+            Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
+            LaserScan bscanHandle;
+            bscanHandle.header = bscanHeader;
+            bscanHandle.angle_min = 1.57f;
+            bscanHandle.angle_max = -1.57f;
+            bscanHandle.angle_increment = -0.785f;
+            bscanHandle.time_increment = 0.0f;
+            bscanHandle.scan_time = 0.1f;
+            bscanHandle.range_min = 0.05f;
+            bscanHandle.range_max = 10.0f;
+            bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5);
+            bscanHandle.ranges_count = new UIntPtr(5);
+            bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5);
+            bscanHandle.intensities_count = new UIntPtr(5);
+            NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan",  bscanHandle);
+
+            IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
+            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;
+            odometryHandle.pose.pose.position.x = 0.0;
+            odometryHandle.pose.pose.position.y = 0.0;
+            odometryHandle.pose.pose.position.z = 0.0;
+            odometryHandle.pose.pose.orientation.x = 0.0;
+            odometryHandle.pose.pose.orientation.y = 0.0;
+            odometryHandle.pose.pose.orientation.z = 0.0;
+            odometryHandle.pose.pose.orientation.w = 1.0;
+            
+            double[] pose_covariance = new double[36];
+            for(int i = 0; i < pose_covariance.Length; i++) {
+                pose_covariance[i] = 0.0;
+            }
+            odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length);
+            Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length);
+            odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length);
+
+            odometryHandle.twist.twist.linear.x = 0.0;
+            odometryHandle.twist.twist.linear.y = 0.0;
+            odometryHandle.twist.twist.linear.z = 0.0;
+            odometryHandle.twist.twist.angular.x = 0.0;
+            odometryHandle.twist.twist.angular.y = 0.0;
+            odometryHandle.twist.twist.angular.z = 0.0;
+            double[] twist_covariance = new double[36];
+            for(int i = 0; i < twist_covariance.Length; i++) {
+                twist_covariance[i] = 0.0;
+            }
+            odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length);
+            Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length);
+            odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length);
+            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 =  "maze.yaml";
+            int mapWidth, mapHeight;
+            byte[] data;
+            MazeMapConfig mapConfig;
+            if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
             {
-                Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
+                data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
+                    mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
+                if (data == null)
+                {
+                    mapWidth = 3;
+                    mapHeight = 10;
+                    data = new byte[30];
+                    for (int i = 0; i < data.Length; i++) data[i] = 100;
+                    Console.WriteLine("YAML loaded but image failed; using default map 3x10");
+                }
+                else
+                {
+                    Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
+                        mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
+                        mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
+                }
+            }
+            else
+            {
+                mapWidth = 3;
+                mapHeight = 10;
+                data = new byte[30];
+                for (int i = 0; i < data.Length; i++) data[i] = 100;
+                mapConfig = default;
+                mapConfig.Resolution = 0.05f;
+                mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
+                Console.WriteLine("maze.yaml not found, using default map 3x10");
             }
 
-            // Get navigation feedback
-            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
-            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
-            {
-                string stateStr = NavigationAPI.MarshalString(
-                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
-                NavigationAPI.navigation_free_feedback(ref feedback);
-            }
+            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 Pose();
+            mapMetaData.origin.position.x = mapConfig.OriginX;
+            mapMetaData.origin.position.y = mapConfig.OriginY;
+            mapMetaData.origin.position.z = mapConfig.OriginZ;
+            mapMetaData.origin.orientation.x = 0.0;
+            mapMetaData.origin.orientation.y = 0.0;
+            mapMetaData.origin.orientation.z = 0.0;
+            mapMetaData.origin.orientation.w = 1.0;
+            OccupancyGrid occupancyGrid = new OccupancyGrid();
+            IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
+            occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
+            occupancyGrid.info = mapMetaData;
+            occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length);
+            Marshal.Copy(data, 0, occupancyGrid.data, data.Length);
+            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<OccupancyGrid>(),
+                Marshal.OffsetOf<OccupancyGrid>("data"),
+                Marshal.OffsetOf<OccupancyGrid>("data_count"));
+    
+            NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
+
+            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);
+            }  
+
+            // 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)
 
-            // Cleanup
             NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
+            TF3API.tf3_buffer_destroy(tf3Buffer);
+            Console.WriteLine("Press any key to exit...");
+            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/QUICK_START.md

@@ -85,9 +85,9 @@ cp ../../build/src/APIs/c_api/libnav_c_api.so .
 
 # 5. Set library path (quan trọng!)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -118,9 +118,9 @@ ls -la ../../build/src/APIs/c_api/libnav_c_api.so
 
 # Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -143,6 +143,6 @@ ldd libnav_c_api.so
 → Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
    Nếu vẫn gặp lỗi, kiểm tra:
    - Library đã được build đầy đủ: `make nav_c_api` trong build directory
-   - Dependencies đã được build: `make move_base costmap_2d`
+   - Dependencies đã được build: `make move_base robot_costmap_2d`
    - LD_LIBRARY_PATH đã được set đúng
 

examples/README.md

@@ -78,9 +78,9 @@ Tạo file `NavigationExample.csproj`:
 ```bash
 # Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH

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!"
 
 
@@ -82,12 +83,10 @@ fi
 
 # Luôn copy source code mới nhất (cập nhật file nếu đã có)
 cd "$EXAMPLE_DIR/NavigationExample"
-echo "Updating Program.cs from CSharpExample.cs..."
-cp ../CSharpExample.cs Program.cs
 
-# 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.)
@@ -95,9 +94,9 @@ export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH"
 # Library directories for dependencies
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/costmap_2d:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"

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

@@ -10,13 +10,20 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-# Dependencies
+# Find Boost (filesystem needed for plugin path / boost::dll usage)
+find_package(Boost REQUIRED COMPONENTS filesystem system)
+
+# Dependencies (robot_nav_2d_utils_conversions provides poseStampedToPose2D used by move_base_core/navigation.h)
 set(PACKAGES_DIR
     move_base_core
-    move_base
+    robot_nav_2d_utils
     tf3
     robot_time
+    robot_cpp
     geometry_msgs
+    angles
+    data_convert
+    robot_protocol_msgs
 )
 
 # Thư mục include
@@ -25,8 +32,28 @@ include_directories(
 )
 
 # Tìm tất cả file source
-file(GLOB SOURCES "src/*.cpp")
-file(GLOB HEADERS "include/*.h")
+file(GLOB SOURCES 
+    "src/*.cpp" 
+    "src/std_msgs/*.cpp" 
+    "src/sensor_msgs/*.cpp" 
+    "src/geometry_msgs/*.cpp"
+    "src/nav_msgs/*.cpp"
+    "src/nav_2d_msgs/*.cpp"
+    "src/robot_nav_msgs/*.cpp"
+    "src/robot_geometry_msgs/*.cpp"
+    "src/map_msgs/*.cpp"
+)
+file(GLOB HEADERS 
+    "include/*.h" 
+    "include/std_msgs/*.h" 
+    "include/sensor_msgs/*.h" 
+    "include/geometry_msgs/*.h"
+    "include/nav_msgs/*.h"
+    "include/nav_2d_msgs/*.h"
+    "include/robot_nav_msgs/*.h"
+    "include/robot_geometry_msgs/*.h"
+    "include/map_msgs/*.h"
+)
 
 # Tạo thư viện shared (.so)
 add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
@@ -35,10 +62,17 @@ add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
 target_link_libraries(nav_c_api
     PUBLIC
         ${PACKAGES_DIR}
+    PRIVATE
+        Boost::boost
+        Boost::filesystem
+        Boost::system
+        dl
 )
 
 set_target_properties(nav_c_api PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  BUILD_RPATH "${CMAKE_BINARY_DIR}"
+  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
 # Set include directories

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

@@ -0,0 +1,206 @@
+#ifndef C_API_CONVERTOR_H
+#define C_API_CONVERTOR_H
+
+// C
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/PointCloud.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "nav_msgs/Odometry.h"
+#include "std_msgs/Header.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/Point.h"
+#include "geometry_msgs/PoseStamped.h"
+#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>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_nav_msgs/Odometry.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#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>
+
+/**
+ * @brief Convert C LaserScan to C++ LaserScan
+ * @param laser_scan C LaserScan
+ * @return C++ LaserScan
+ */
+robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan);
+
+/**
+ * @brief Convert C OccupancyGrid to C++ OccupancyGrid
+ * @param occupancy_grid C OccupancyGrid
+ * @return C++ OccupancyGrid
+ */
+robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid);
+
+/**
+ * @brief Convert C++ OccupancyGrid to C OccupancyGrid
+ * @param cpp C++ OccupancyGrid
+ * @param out C OccupancyGrid
+ */
+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
+ * @param out C LaserScan
+ */
+void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out);
+
+/**
+ * @brief Convert C++ PointCloud to C PointCloud
+ * @param cpp C++ PointCloud
+ * @param out C PointCloud
+ */
+void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out);
+
+/**
+ * @brief Convert C++ PointCloud2 to C PointCloud2
+ * @param cpp C++ PointCloud2
+ * @param out C PointCloud2
+ */
+void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out);
+
+/**
+ * @brief Convert C PointCloud to C++ PointCloud
+ * @param c C PointCloud
+ * @return C++ PointCloud
+ */
+robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c);
+
+/**
+ * @brief Convert C PointCloud2 to C++ PointCloud2
+ * @param c C PointCloud2
+ * @return C++ PointCloud2
+ */
+robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c);
+
+/**
+ * @brief Convert C Odometry to C++ Odometry
+ * @param odometry C Odometry
+ * @return C++ Odometry
+ */
+robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry);
+
+/**
+ * @brief Convert C PoseStamped to C++ PoseStamped
+ * @param pose_stamped C PoseStamped
+ * @return C++ PoseStamped
+ */
+robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& pose_stamped);
+
+/**
+ * @brief Convert C++ PoseStamped to C PoseStamped
+ * @param cpp_pose_stamped C++ PoseStamped
+ * @return C PoseStamped
+ */
+PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose_stamped);
+
+
+/**
+ * @brief Convert C Pose2D to C++ Pose2D
+ * @param pose_2d C Pose2D
+ * @return C++ Pose2D
+ */
+robot_geometry_msgs::Pose2D convert2CppPose2D(const Pose2D& pose_2d);
+
+/**
+ * @brief Convert C++ Pose2D to C Pose2D
+ * @param cpp_pose_2d C++ Pose2D
+ * @return C Pose2D
+ */
+ Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose_2d);
+
+/**
+ * @brief Convert C Twist2D to C++ Twist2D
+ * @param twist_2d C Twist2D
+ * @return C++ Twist2D
+ */
+robot_nav_2d_msgs::Twist2D convert2CppTwist2D(const Twist2D& twist_2d);
+
+
+/**
+ * @brief Convert C++ Twist2D to C Twist2D
+ * @param cpp_twist_2d C++ Twist2D
+ * @return C Twist2D
+ */
+Twist2D convert2CTwist2D(const robot_nav_2d_msgs::Twist2D& cpp_twist_2d);
+
+/**
+ * @brief Convert C Twist2DStamped to C++ Twist2DStamped
+ * @param twist_2d_stamped C Twist2DStamped
+ * @return C++ Twist2DStamped
+ */
+robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped& twist_2d_stamped);
+
+
+/**
+ * @brief Convert C++ Twist2DStamped to C Twist2DStamped
+ * @param cpp_twist_2d_stamped C++ Twist2DStamped
+ * @return C 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/geometry_msgs/Accel.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCEL_H
+#define C_API_GEOMETRY_MSGS_ACCEL_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 linear;
+  Vector3 angular;
+} Accel;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCEL_H

src/APIs/c_api/include/geometry_msgs/AccelStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELSTAMPED_H
+#define C_API_GEOMETRY_MSGS_ACCELSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Accel.h"
+
+typedef struct
+{
+  Header header;
+  Accel accel;
+} AccelStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELSTAMPED_H

src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Accel.h"
+
+typedef struct
+{
+  Accel accel;
+  double covariance[36];
+} AccelWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/AccelWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  AccelWithCovariance accel;
+} AccelWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Inertia.h

@@ -0,0 +1,28 @@
+#ifndef C_API_GEOMETRY_MSGS_INERTIA_H
+#define C_API_GEOMETRY_MSGS_INERTIA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  double m;
+  Vector3 com;
+  double ixx;
+  double ixy;
+  double ixz;
+  double iyy;
+  double iyz;
+  double izz;
+} Inertia;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_INERTIA_H

src/APIs/c_api/include/geometry_msgs/InertiaStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_INERTIASTAMPED_H
+#define C_API_GEOMETRY_MSGS_INERTIASTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Inertia.h"
+
+typedef struct
+{
+  Header header;
+  Inertia inertia;
+} InertiaStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_INERTIASTAMPED_H

src/APIs/c_api/include/geometry_msgs/Point.h

@@ -0,0 +1,21 @@
+#ifndef C_API_GEOMETRY_MSGS_POINT_H
+#define C_API_GEOMETRY_MSGS_POINT_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+#include <stdint.h>
+#include <stddef.h>
+
+  typedef struct
+  {
+    double x;
+    double y;
+    double z;
+  } Point;
+
+#ifdef __cplusplus
+}
+#endif
+#endif // C_API_GEOMETRY_MSGS_POINT_H

src/APIs/c_api/include/geometry_msgs/Point32.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_POINT32_H
+#define C_API_GEOMETRY_MSGS_POINT32_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  float x;
+  float y;
+  float z;
+} Point32;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POINT32_H

src/APIs/c_api/include/geometry_msgs/PointStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POINTSTAMPED_H
+#define C_API_GEOMETRY_MSGS_POINTSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point.h"
+
+typedef struct
+{
+  Header header;
+  Point point;
+} PointStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POINTSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Polygon.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_POLYGON_H
+#define C_API_GEOMETRY_MSGS_POLYGON_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Point32.h"
+
+typedef struct
+{
+  Point32 *points;
+  size_t points_count;
+} Polygon;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POLYGON_H

src/APIs/c_api/include/geometry_msgs/PolygonStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H
+#define C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Polygon.h"
+
+typedef struct
+{
+  Header header;
+  Polygon polygon;
+} PolygonStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Pose.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSE_H
+#define C_API_GEOMETRY_MSGS_POSE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Point.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Point position;
+  Quaternion orientation;
+} Pose;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSE_H

src/APIs/c_api/include/geometry_msgs/Pose2D.h

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

src/APIs/c_api/include/geometry_msgs/PoseArray.h

@@ -0,0 +1,24 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEARRAY_H
+#define C_API_GEOMETRY_MSGS_POSEARRAY_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Header header;
+  Pose *poses;
+  size_t poses_count;
+} PoseArray;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEARRAY_H

src/APIs/c_api/include/geometry_msgs/PoseStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSESTAMPED_H
+#define C_API_GEOMETRY_MSGS_POSESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Header header;
+  Pose pose;
+} PoseStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSESTAMPED_H

src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Pose pose;
+  double *covariance;
+  size_t covariance_count;
+} PoseWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  PoseWithCovariance pose;
+} PoseWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Quaternion.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_QUATERNION_H
+#define C_API_GEOMETRY_MSGS_QUATERNION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double z;
+  double w;
+} Quaternion;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_QUATERNION_H

src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H
+#define C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Header header;
+  Quaternion quaternion;
+} QuaternionStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Transform.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TRANSFORM_H
+#define C_API_GEOMETRY_MSGS_TRANSFORM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Vector3 translation;
+  Quaternion rotation;
+} Transform;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TRANSFORM_H

src/APIs/c_api/include/geometry_msgs/TransformStamped.h

@@ -0,0 +1,24 @@
+#ifndef C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H
+#define C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Transform.h"
+
+typedef struct
+{
+  Header header;
+  char *child_frame_id;
+  Transform transform;
+} TransformStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Twist.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_TWIST_H
+#define C_API_GEOMETRY_MSGS_TWIST_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 linear;
+  Vector3 angular;
+} Twist;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWIST_H

src/APIs/c_api/include/geometry_msgs/TwistStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTSTAMPED_H
+#define C_API_GEOMETRY_MSGS_TWISTSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Twist.h"
+
+typedef struct
+{
+  Header header;
+  Twist twist;
+} TwistStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTSTAMPED_H

src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Twist.h"
+
+typedef struct
+{
+  Twist twist;
+  double *covariance;
+  size_t covariance_count;
+} TwistWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/TwistWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  TwistWithCovariance twist;
+} TwistWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Vector3.h

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

src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H
+#define C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Header header;
+  Vector3 vector;
+} Vector3Stamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H

src/APIs/c_api/include/geometry_msgs/Wrench.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_WRENCH_H
+#define C_API_GEOMETRY_MSGS_WRENCH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 force;
+  Vector3 torque;
+} Wrench;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_WRENCH_H

src/APIs/c_api/include/geometry_msgs/WrenchStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H
+#define C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Wrench.h"
+
+typedef struct
+{
+  Header header;
+  Wrench wrench;
+} WrenchStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H

src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h

@@ -0,0 +1,33 @@
+#ifndef C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+#define C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+
+/**
+ * C representation of robot_map_msgs::OccupancyGridUpdate.
+ * Note: This header is intentionally named *_c.h to avoid
+ * shadowing the C++ header `robot_map_msgs/OccupancyGridUpdate.h`.
+ */
+typedef struct
+{
+  Header header;
+  int32_t x;
+  int32_t y;
+  uint32_t width;
+  uint32_t height;
+  int8_t *data;
+  size_t data_count;
+} OccupancyGridUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+

src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H
+#define C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/Polygon2D.h"
+
+typedef struct
+{
+  Polygon2D outer;
+  Polygon2D *inner;
+  size_t inner_count;
+} ComplexPolygon2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H

src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDINFO_H
+#define C_API_NAV_2D_MSGS_NAVGRIDINFO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint32_t width;
+  uint32_t height;
+  double resolution;
+  char *frame_id;
+  double origin_x;
+  double origin_y;
+} NavGridInfo;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDINFO_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/NavGridInfo.h"
+
+typedef struct
+{
+  Time stamp;
+  NavGridInfo info;
+  uint8_t *data;
+  size_t data_count;
+} NavGridOfChars;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/UIntBounds.h"
+
+typedef struct
+{
+  Time stamp;
+  UIntBounds bounds;
+  uint8_t *data;
+  size_t data_count;
+} NavGridOfCharsUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/NavGridInfo.h"
+
+typedef struct
+{
+  Time stamp;
+  NavGridInfo info;
+  double *data;
+  size_t data_count;
+} NavGridOfDoubles;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/UIntBounds.h"
+
+typedef struct
+{
+  Time stamp;
+  UIntBounds bounds;
+  double *data;
+  size_t data_count;
+} NavGridOfDoublesUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H

src/APIs/c_api/include/nav_2d_msgs/Path2D.h

@@ -0,0 +1,24 @@
+#ifndef C_API_NAV_2D_MSGS_PATH2D_H
+#define C_API_NAV_2D_MSGS_PATH2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_2d_msgs/Pose2DStamped.h"
+
+typedef struct
+{
+  Header header;
+  Pose2DStamped *poses;
+  size_t poses_count;
+} Path2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_PATH2D_H

src/APIs/c_api/include/nav_2d_msgs/Point2D.h

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

src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2D_H
+#define C_API_NAV_2D_MSGS_POLYGON2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/Point2D.h"
+
+typedef struct
+{
+  Point2D *points;
+  size_t points_count;
+} Polygon2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2D_H

src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h

@@ -0,0 +1,27 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H
+#define C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "std_msgs/ColorRGBA.h"
+#include "nav_2d_msgs/ComplexPolygon2D.h"
+
+typedef struct
+{
+  Header header;
+  ComplexPolygon2D *polygons;
+  size_t polygons_count;
+  ColorRGBA *colors;
+  size_t colors_count;
+} Polygon2DCollection;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H

src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H
+#define C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_2d_msgs/Polygon2D.h"
+
+typedef struct
+{
+  Header header;
+  Polygon2D polygon;
+} Polygon2DStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H

src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_POSE2D32_H
+#define C_API_NAV_2D_MSGS_POSE2D32_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  float x;
+  float y;
+  float theta;
+} Pose2D32;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POSE2D32_H

src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_POSE2DSTAMPED_H
+#define C_API_NAV_2D_MSGS_POSE2DSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose2D.h"
+
+typedef struct
+{
+  Header header;
+  Pose2D pose;
+} Pose2DStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POSE2DSTAMPED_H

src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGIN_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGIN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/SwitchPluginRequest.h"
+#include "nav_2d_msgs/SwitchPluginResponse.h"
+
+typedef struct
+{
+  SwitchPluginRequest request;
+  SwitchPluginResponse response;
+} SwitchPlugin;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGIN_H

src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h

@@ -0,0 +1,20 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *new_plugin;
+} SwitchPluginRequest;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H