fix load path so

.gitignore

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

.gitmodules

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

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,20 +74,24 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
-if (NOT TARGET nav_core)
+if (NOT TARGET laser_filter)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
+    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)
+if (NOT TARGET robot_nav_core2)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core2)
 endif()
 
-if (NOT TARGET nav_core_adapter)
+if (NOT TARGET robot_nav_core_adapter)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core_adapter)
 endif()
 
 if (NOT TARGET move_base_core)
@@ -138,6 +138,17 @@ 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()
 
 # 2. Main packages (phụ thuộc vào cores)
 # message(STATUS "[move_base] Shared library configured")

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,11 @@
+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
+

config/costmap_common_params.yaml

@@ -1,4 +1,4 @@
-robot_base_frame: base_footprint
+robot_base_frame: base_link
 transform_tolerance: 1.0
 obstacle_range: 3.0
 #mark_threshold: 1
@@ -25,7 +25,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   f_scan_clearing:
@@ -33,7 +33,7 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_marking:
@@ -41,7 +41,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_clearing:
@@ -49,6 +49,8 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
+
+

config/costmap_global_params.yaml

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

config/costmap_local_params.yaml

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

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/move_base_common_params.yaml

@@ -1,17 +1,39 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PNKXLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
-controller_frequency: 20.0   # run controller at 15.0 Hz
+controller_frequency: 30.0   # run controller at 15.0 Hz
 controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
-planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+planner_patience: 2.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)
+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
+oscillation_distance: 0.5
 ### recovery behaviors
 recovery_behavior_enabled: true
 recovery_behaviors: [       
-                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
+                      {name: aggressive_reset, type: ClearCostmapRecovery},
-                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
+                      {name: conservative_reset, type: ClearCostmapRecovery},                      
                     ]
 
 conservative_reset:
@@ -21,5 +43,8 @@ conservative_reset:
 aggressive_reset:
   reset_distance: 3.0
 
+ClearCostmapRecovery:
+  library_path: librobot_clear_costmap_recovery
+
 MoveBase:
   library_path: libmove_base

config/mprim/ekf.yaml

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

config/mprim/localization.yaml

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

config/mprim/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.03  
+xy_goal_tolerance: 0.02 
+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,15 +48,15 @@ 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
+  max_vel_theta: 0.4   # 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
+  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: 1.5
   acc_lim_y: 0.0      # diff drive robot
-  acc_lim_theta: 0.9
+  acc_lim_theta: 1.5
-  decel_lim_x: -1.0
+  decel_lim_x: -1.5
   decel_lim_y: -0.0
   decel_lim_theta: -1.5
 
@@ -81,22 +74,30 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.02
-  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.47
-  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)
+  lookahead_time: 1.9                        # 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) 
+  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_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,26 +106,21 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.1
+  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.1
+  trans_stopped_velocity: 0.06
   
-  # Regulated linear velocity scaling
+  use_final_heading_alignment: true
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  final_heading_xy_tolerance: 0.1
-  # only when true:
+  final_heading_angle_tolerance: 0.05
-  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)
+  final_heading_min_velocity: 0.05
-  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)
+  final_heading_kp_angular: 1.2
-
+  final_heading_ki_angular: 0.002
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  final_heading_kd_angular: 0.12
-  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)
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.02
-  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.8
-  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 
@@ -133,42 +129,12 @@ MKTAlgorithmDiffGoStraight:
   # 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)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  max_lookahead_dist: 2.0                     # 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)
+  lookahead_time: 2.0                        # 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) 
+  min_journey_squared: 1.0                    # 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) 
+  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.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
-  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)
-  # 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) 
 
   # 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)
@@ -177,24 +143,57 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.03
+  rot_stopped_velocity: 0.05
   trans_stopped_velocity: 0.06
   
-  # Regulated linear velocity scaling
+  use_final_heading_alignment: true
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: 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.02
+  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)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  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)
+  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)
 
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  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)
+  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)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
+  # only when true:
-  cost_scaling_dist: 0.2                             # (default: 0.6)
+  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)
-  cost_scaling_gain: 2.0                             # (default: 1.0)
+  angular_decel_zone: 0.1             
 
+  # stoped
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
+  
+  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/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

@@ -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.)
+   - `robot_nav_core::BaseGlobalPlanner`: Interface cho global planners
-   - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
+   - **Global Planner Implementations**:
-   - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+     - `custom_planner` - Custom path planner với curve support và merge path calculation
-   - Plugin system sử dụng `boost::dll` để dynamic loading
+     - `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** ✅
-   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap wrapper
-   - Costmap layers: static map, obstacles, inflation
+   - `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
+   - **MKT Algorithm** (`mkt_algorithm`):
-   - `score_algorithm`: Trajectory scoring và goal checking
+     - Differential drive kinematics:
-   - `kalman`: Filtering algorithms
+       - `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)
@@ -250,19 +294,23 @@ 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` ✅
+- `robot_nav_core::BaseLocalPlanner` ✅
-  - `computeVelocityCommands(cmd_vel)` - Tính toán velocity command
+  - `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
 
 - `robot_costmap_2d::Costmap2DROBOT` ✅
   - Wrapper cho costmap với robot footprint
@@ -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
+1. ✅ **Interface Layer**: 
-2. ✅ **Implementation Layer**: `MoveBase` core logic
+   - `move_base_core::BaseNavigation` - Abstract interface hoàn chỉnh
-3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
+   - `move_base_core::common.h` - Common types và utilities
-4. ✅ **Costmap Layer**: Global và local costmap với layers
+
-5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
+2. ✅ **Implementation Layer**: 
-6. ✅ **API Layer**: C API wrapper cho .NET integration
+   - `move_base::MoveBase` - Core implementation với state machine
-7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, robot_nav_2d_utils
+   - `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
+   - `ILocalizationSource` interface - Cần định nghĩa
-   - `IOdometrySource` interface
+   - `IOdometrySource` interface - Cần định nghĩa
-   - `IMapProvider` interface
+   - `IMapProvider` interface - Cần định nghĩa
-   - Integration với Pnkx Loc, odometry sources
+   - 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
+   - `IBaseController` interface - Cần định nghĩa
-   - Diff drive controller implementation
+   - Diff drive controller implementation - Cần implement
-   - Steer drive controller implementation
+   - Steer drive controller implementation - Cần implement
-   - Velocity command execution
+   - Velocity command execution - Hiện tại chỉ có `setTwistLinear/Angular` trong local planner
 
-3. ⚠️ **Control Loop**:
+3. ⚠️ **User Controller Plugin System**:
-   - Control loop trong MoveBase (executeCycle)
+   - Factory để load user controller plugins - Cần implement
-   - State machine management hoàn chỉnh
+   - Interface cho User Controller - Cần định nghĩa
-   - Threading và synchronization
+   - Integration với BaseNavigation - Cần implement
 
-4. ⚠️ **User Controller Plugin System**:
+4. ⚠️ **Testing & Documentation**:
-   - Factory để load user controller plugins
+   - Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
-   - Integration với BaseNavigation
+   - 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
+│   │   │   ├── move_base_core/              # BaseNavigation interface ✅
-│   │   │   ├── nav_core/            # Planner interfaces
+│   │   │   │   ├── include/move_base_core/
-│   │   │   ├── nav_core_adapter/    # Adapter utilities
+│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
-│   │   │   └── nav_core2/           # Additional nav utilities
+│   │   │   │   │   └── 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/
-│   │   │   ├── robot_costmap_2d/          # Costmap system
+│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
-│   │   │   ├── tf3/                 # Transform system
+│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
-│   │   │   ├── robot_time/          # Time management
-│   │   │   ├── geometry2/           # Geometry utilities
-│   │   │   └── ...                  # Other supporting 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
+│   │   │   └── score_algorithm/             # Trajectory scoring ✅
-│   │   └── Libraries/
+│   │   │       ├── include/score_algorithm/
-│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
+│   │   │       │   ├── score_algorithm.h
-│   │       ├── kalman/              # Kalman filtering
+│   │   │       │   ├── goal_checker.h
-│   │       └── angles/               # Angle utilities
+│   │   │       │   └── 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
+│       └── 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/
+Mô tả cấu trúc thư mục thực tế của dự án:
-│   ├── build/
+
-│   ├── geometry_msgs/
+```
-│   │   ├── include/
+pnkx_nav_core/
-│   │   └── test/
+├── src/
-│   ├── CMakeLists.txt
+│   ├── Navigations/
-│   ├── robot_nav_msgs/
+│   │   ├── Cores/
-│   │   ├── include/
+│   │   │   ├── move_base_core/              # BaseNavigation interface
-│   │   └── test/
+│   │   │   ├── robot_nav_core/              # Planner interfaces (BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior)
-│   ├── CMakeLists.txt
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities (global_planner_adapter, local_planner_adapter, costmap_adapter)
-│   ├── robot_sensor_msgs/
+│   │   │   └── robot_nav_core2/             # Additional nav utilities (global_planner, local_planner, costmap)
-│   │   ├── cfg/
+│   │   ├── Libraries/
-│   │   ├── include/
+│   │   │   └── nav_grid/                    # Navigation grid utilities
-│   │   └── test/
+│   │   └── Packages/
-│   ├── CMakeLists.txt
+│   │       └── move_base/                   # MoveBase implementation
-│   ├── robot_std_msgs/
+│   │
-│   │   ├── include/
+│   ├── Algorithms/
-│   │   └── CMakeLists.txt
+│   │   ├── Cores/
-│   └── CMakeLists.txt (root)
+│   │   │   └── score_algorithm/             # Trajectory scoring và goal checking
-|
+│   │   ├── Libraries/
-├── Navigations/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics (diff drive & bicycle)
-│   ├── Cores/
+│   │   │   ├── mkt_plugins/                 # MKT plugin components
-│   │   └── move_base_core/
+│   │   │   ├── mkt_msgs/                    # MKT message types
-│   │       ├── build/
+│   │   │   ├── kalman/                      # Kalman filtering
-│   │       ├── example/
+│   │   │   └── angles/                      # Angle utilities
-│   │       ├── include/
+│   │   └── Packages/
-│   │       ├── .gitignore
+│   │       ├── global_planners/
-│   │       ├── CMakeLists.txt
+│   │       │   ├── custom_planner/           # Custom global planner
-│   │       └── README.md
+│   │       │   ├── dock_planner/            # Docking planner
+│   │       │   └── two_points_planner/      # Two points planner
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner
+│   │
 │   ├── Libraries/
-│   │   ├── geometry_msgs/
+│   │   ├── costmap_2d/                      # Costmap system với 8 layers
-│   │   │   ├── build/
+│   │   ├── tf3/                             # Transform system
-│   │   │   ├── include/
+│   │   ├── robot_time/                      # Time management
-│   │   │   └── CMakeLists.txt
+│   │   ├── robot_cpp/                       # Core utilities (init, NodeHandle, PluginLoaderHelper)
-│   │   └── libtf2/
+│   │   ├── robot_nav_2d_utils/             # 2D navigation utilities
-│   │       ├── .vscode/
+│   │   ├── robot_nav_2d_msgs/              # 2D navigation messages
-│   │       ├── include/
+│   │   ├── geometry2/                       # Geometry utilities (tf3_geometry_msgs, tf3_sensor_msgs)
-│   │       ├── samples/
+│   │   ├── common_msgs/                     # Message types
-│   │       ├── src/
+│   │   │   ├── robot_geometry_msgs/         # Geometry messages (29 types)
-│   │       ├── .gitignore
+│   │   │   ├── robot_nav_msgs/             # Navigation messages (24 types)
-│   │       ├── DEBIAN_PACKAGING.md
+│   │   │   ├── robot_sensor_msgs/          # Sensor messages (30 types)
-│   │       ├── libtf2_2.0.0_amd64.deb
+│   │   │   ├── robot_std_msgs/             # Standard messages (32 types)
-│   │       ├── Makefile
+│   │   │   ├── robot_protocol_msgs/        # Protocol messages
-│   │       └── README.md
+│   │   │   ├── robot_visualization_msgs/   # Visualization messages
-│   ├── Packages/
+│   │   │   └── utils/                       # Message utilities
-│   │   └── move_base/
+│   │   ├── laser_geometry/                  # Laser geometry utilities
-│   │       ├── build/
+│   │   ├── voxel_grid/                      # Voxel grid
-│   │       ├── include/
+│   │   ├── data_convert/                    # Data conversion
-│   │       ├── src/
+│   │   └── xmlrpcpp/                        # XML-RPC utilities
-│   │       └── CMakeLists.txt
+│   │
-│   └── CMakeLists.txt (root)
+│   └── 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.

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>
+    <TargetFramework>net10.0</TargetFramework>
-    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
     <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>
+    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
-    /// C# P/Invoke wrapper for Navigation C API
+    public struct MazeMapConfig
-    /// </summary>
-    public class NavigationAPI
     {
-        private const string DllName = "libnav_c_api.so"; // Linux
+        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
-        // For Windows: "nav_c_api.dll"
+        public float Resolution;
-        // For macOS: "libnav_c_api.dylib"
+        public double OriginX, OriginY, OriginZ;
-
+        public int Negate;            // 0 hoặc 1
-        // ============================================================================
+        public double OccupiedThresh;
-        // Enums
+        public double FreeThresh;
-        // ============================================================================
-        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);
-        }
     }
 
     // ============================================================================
@@ -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
+            // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
-            IntPtr tfHandle = NavigationAPI.tf_listener_create();
+            IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
-            if (tfHandle == IntPtr.Zero)
+            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).
+            // Create navigation instance and initialize with tf3 buffer
-            // If you already publish TF from localization/odometry, you can remove this call.
+            NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
-            if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
+            if (navHandle.ptr == IntPtr.Zero)
-                                                               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);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Initialize navigation
+            if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
             {
-                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;
             }
 
-            // Set robot footprint
+            while (true)
-            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(
+                    if (feedback.is_ready)
-                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
+                    {
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
+                        Console.WriteLine("Navigation is ready");
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
+                        break;
-                NavigationAPI.navigation_free_feedback(ref feedback);
+                    }
+                    else
+                    {
+                        Console.WriteLine("Navigation is not ready");
+                    }
+                }
+                System.Threading.Thread.Sleep(100);
+            }
+            Console.WriteLine("[NavigationExample] Navigation initialized successfully");
+
+
+            // Set robot footprint
+            Point[] footprint = new Point[]
+            {
+                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));
+
+  
+            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))
+            {
+                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");
             }
 
-            // Cleanup
+            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)
+
             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/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
+# # Bước 3: Copy library
-echo "Copying library..."
+# echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 # Main build directory (contains libtf3.so, librobot_cpp.so, etc.)

pnkx_nav_core.sln

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

src/APIs/c_api/CMakeLists.txt

@@ -10,17 +10,20 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-# Find Boost
+# Find Boost (filesystem needed for plugin path / boost::dll usage)
-find_package(Boost REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem system)
 
-# Dependencies
+# 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
@@ -29,8 +32,28 @@ include_directories(
 )
 
 # Tìm tất cả file source
-file(GLOB SOURCES "src/*.cpp")
+file(GLOB SOURCES 
-file(GLOB HEADERS "include/*.h")
+    "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})
@@ -41,11 +64,15 @@ target_link_libraries(nav_c_api
         ${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/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/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

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

@@ -0,0 +1,21 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint8_t success;
+  char *message;
+} SwitchPluginResponse;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H

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

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

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

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

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

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

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

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_UINTBOUNDS_H
+#define C_API_NAV_2D_MSGS_UINTBOUNDS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint32_t min_x;
+  uint32_t min_y;
+  uint32_t max_x;
+  uint32_t max_y;
+} UIntBounds;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_UINTBOUNDS_H

src/APIs/c_api/include/nav_c_api.h

@@ -2,121 +2,57 @@
 #define NAVIGATION_C_API_H
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
 
   #include <stdbool.h>
   #include <stdint.h>
   #include <stddef.h>
+  /* C struct types only - do not include convertor.h here (it pulls C++ code into extern "C") */
+  #include "std_msgs/Header.h"
+  #include "geometry_msgs/Point.h"
+  #include "geometry_msgs/Pose2D.h"
+  #include "geometry_msgs/PoseStamped.h"
+  #include "nav_msgs/OccupancyGrid.h"
+  #include "nav_msgs/Odometry.h"
+  #include "map_msgs/OccupancyGridUpdate.h"
+  #include "nav_2d_msgs/Twist2D.h"
+  #include "nav_2d_msgs/Twist2DStamped.h"
+  #include "nav_2d_msgs/Path2D.h"
+  #include "sensor_msgs/LaserScan.h"
+  #include "sensor_msgs/PointCloud.h"
+  #include "sensor_msgs/PointCloud2.h"
+  #include "geometry_msgs/PolygonStamped.h"
+  #include "protocol_msgs/Order.h"
 
-// Forward declarations
+  typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid;
+  typedef struct { char *name; LaserScan scan; } NamedLaserScan;
+  typedef struct { char *name; PointCloud cloud; } NamedPointCloud;
+  typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2;
   typedef void* NavigationHandle;
   typedef void* TFListenerHandle;
   
-// ============================================================================
+  typedef enum
-// Enums
+  {
-// ============================================================================
+    PENDING = 0,
-
+    ACTIVE = 1,
-/**
+    PREEMPTED = 2,
- * @brief Navigation states, including planning and controller status
+    SUCCEEDED = 3,
- */
+    ABORTED = 4,
-typedef enum {
+    REJECTED = 5,
-    NAV_STATE_PENDING = 0,
+    PREEMPTING = 6,
-    NAV_STATE_ACTIVE = 1,
+    RECALLING = 7,
-    NAV_STATE_PREEMPTED = 2,
+    RECALLED = 8,
-    NAV_STATE_SUCCEEDED = 3,
+    LOST = 9,
-    NAV_STATE_ABORTED = 4,
+    PLANNING = 10,
-    NAV_STATE_REJECTED = 5,
+    CONTROLLING = 11,
-    NAV_STATE_PREEMPTING = 6,
+    CLEARING = 12,
-    NAV_STATE_RECALLING = 7,
+    PAUSED = 13
-    NAV_STATE_RECALLED = 8,
-    NAV_STATE_LOST = 9,
-    NAV_STATE_PLANNING = 10,
-    NAV_STATE_CONTROLLING = 11,
-    NAV_STATE_CLEARING = 12,
-    NAV_STATE_PAUSED = 13
   } NavigationState;
   
-// ============================================================================
+  typedef struct
-// Structures
+  {
-// ============================================================================
-
-/**
- * @brief Point structure (x, y, z)
- */
-typedef struct {
-    double x;
-    double y;
-    double z;
-} Point;
-
-/**
- * @brief Pose2D structure (x, y, theta)
- */
-typedef struct {
-    double x;
-    double y;
-    double theta;
-} Pose2D;
-
-/**
- * @brief Quaternion structure
- */
-typedef struct {
-    double x;
-    double y;
-    double z;
-    double w;
-} Quaternion;
-
-/**
- * @brief Position structure
- */
-typedef struct {
-    double x;
-    double y;
-    double z;
-} Position;
-
-/**
- * @brief Pose structure
- */
-typedef struct {
-    Position position;
-    Quaternion orientation;
-} Pose;
-
-/**
- * @brief Header structure
- */
-typedef struct {
-    uint32_t seq;
-    int64_t sec;
-    uint32_t nsec;
-    char* frame_id;
-} Header;
-
-/**
- * @brief PoseStamped structure
- */
-typedef struct {
-    Header header;
-    Pose pose;
-} PoseStamped;
-
-/**
- * @brief Vector3 structure
- */
-typedef struct {
-    double x;
-    double y;
-    double z;
-} Vector3;
-
-/**
- * @brief Navigation feedback structure
- */
-typedef struct {
     NavigationState navigation_state;
     char *feed_back_str;
     Pose2D current_pose;
@@ -124,59 +60,25 @@ typedef struct {
     bool is_ready;
   } NavFeedback;
 
-// ============================================================================
+  /**
-// String Management
+   * @brief Planner data output structure (C version).
-// ============================================================================
+   * Mirrors robot::move_base_core::PlannerDataOutput in `move_base_core/navigation.h`.
+   */
+  typedef struct
+  {
+    Path2D plan;
+    OccupancyGrid costmap;
+    OccupancyGridUpdate costmap_update;
+    bool is_costmap_updated;
+    PolygonStamped footprint;
+  } PlannerDataOutput;
 
   /**
- * @brief Free a string allocated by the library
+   * @brief Free a string allocated by the API (e.g. feed_back_str, navigation_state_to_string result).
- * @param str String to free
+   * @param str Pointer from strdup; no-op if NULL.
    */
   void nav_c_api_free_string(char *str);
 
-// ============================================================================
-// State Conversion
-// ============================================================================
-
-/**
- * @brief Convert a State enum to its string representation
- * @param state Enum value of NavigationState
- * @return String representation (caller must free with nav_c_api_free_string)
- */
-char* navigation_state_to_string(NavigationState state);
-
-// ============================================================================
-// Helper Functions
-// ============================================================================
-
-/**
- * @brief Creates a target pose by offsetting a given 2D pose along its heading direction
- * @param pose_x X coordinate of the original pose
- * @param pose_y Y coordinate of the original pose
- * @param pose_theta Heading angle in radians
- * @param frame_id The coordinate frame ID (null-terminated string)
- * @param offset_distance Distance to offset along heading (positive = forward, negative = backward)
- * @param out_goal Output parameter for the offset pose
- * @return true on success, false on failure
- */
-bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
-                               const char* frame_id, double offset_distance,
-                               PoseStamped* out_goal);
-
-/**
- * @brief Creates an offset target pose from a given PoseStamped
- * @param in_pose Input pose
- * @param offset_distance Distance to offset along heading direction
- * @param out_goal Output parameter for the offset pose
- * @return true on success, false on failure
- */
-bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
-                                     PoseStamped* out_goal);
-
-// ============================================================================
-// Navigation Handle Management
-// ============================================================================
-
   /**
    * @brief Create a new navigation instance
    * @return Navigation handle, or NULL on failure
@@ -189,57 +91,18 @@ NavigationHandle navigation_create(void);
    */
   void navigation_destroy(NavigationHandle handle);
 
-// ============================================================================
-// TF Listener Management
-// ============================================================================
-
-/**
- * @brief Create a TF listener instance
- * @return TF listener handle, or NULL on failure
- */
-TFListenerHandle tf_listener_create(void);
-
-/**
- * @brief Destroy a TF listener instance
- * @param handle TF listener handle to destroy
- */
-void tf_listener_destroy(TFListenerHandle handle);
-
-/**
- * @brief Inject a static transform into the TF buffer.
- *
- * This is a convenience for standalone usage where no external TF publisher exists yet.
- * It will create/ensure the frames exist and become transformable.
- *
- * @param tf_handle TF listener handle
- * @param parent_frame Parent frame id (e.g. "map")
- * @param child_frame Child frame id (e.g. "base_link")
- * @param x Translation x (meters)
- * @param y Translation y (meters)
- * @param z Translation z (meters)
- * @param qx Rotation quaternion x
- * @param qy Rotation quaternion y
- * @param qz Rotation quaternion z
- * @param qw Rotation quaternion w
- * @return true on success, false on failure
- */
-bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
-                                     const char* parent_frame,
-                                     const char* child_frame,
-                                     double x, double y, double z,
-                                     double qx, double qy, double qz, double qw);
-
   // ============================================================================
   // Navigation Interface Methods
   // ============================================================================
 
   /**
- * @brief Initialize the navigation system
+   * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create).
+   * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy).
    * @param handle Navigation handle
- * @param tf_handle TF listener handle
+   * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3)
    * @return true on success, false on failure
    */
-bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer);
 
   /**
    * @brief Set the robot's footprint (outline shape)
@@ -250,49 +113,93 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
    */
   bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count);
 
+  /**
+   * @brief Get the robot's footprint (outline shape)
+   * @param handle Navigation handle
+   * @param out_points Output array of points (allocated by library, free with navigation_free_points)
+   * @param out_count Output number of points in the array
+   * @return true on success, false on failure
+   */
+  bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count);
+
+
   /**
    * @brief Send a goal for the robot to navigate to
    * @param handle Navigation handle
    * @param goal Target pose in the global frame
- * @param xy_goal_tolerance Acceptable error in X/Y (meters)
- * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if goal was accepted and sent successfully
    */
-bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal);
-                        double xy_goal_tolerance, double yaw_goal_tolerance);
+
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   * @note If order was obtained from convert2COrder(), call order_free(&order) when done
+   */
+  bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
+
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param marker Marker name or ID (null-terminated string)
    * @param goal Target pose for docking
- * @param xy_goal_tolerance Acceptable XY error (meters)
- * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-bool navigation_dock_to(NavigationHandle handle, const char* marker,
+  bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal);
-                        const PoseStamped* goal,
+
-                        double xy_goal_tolerance, double yaw_goal_tolerance);
+  /**
+   * @brief Send a docking goal to a predefined marker
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose for docking
+   * @return true if docking command succeeded
+   */
+  bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
+
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+   bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
 
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle
- * @param goal Target pose
+   * @param distance Distance to move (meters)
- * @param xy_goal_tolerance Acceptable positional error (meters)
    * @return true if command issued successfully
    */
-bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_move_straight_to(NavigationHandle handle, const double distance);
-                                  double xy_goal_tolerance);
 
   /**
    * @brief Rotate in place to align with target orientation
    * @param handle Navigation handle
- * @param goal Pose containing desired heading (only Z-axis used)
+   * @param goal_yaw Desired heading (radians)
- * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if rotation command was sent successfully
    */
-bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal);
-                         double yaw_goal_tolerance);
 
   /**
    * @brief Pause the robot's movement
@@ -340,7 +247,7 @@ bool navigation_set_twist_angular(NavigationHandle handle,
    * @param out_pose Output parameter with the robot's current pose
    * @return true if pose was successfully retrieved
    */
-bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose);
+  bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose);
 
   /**
    * @brief Get the robot's pose as a 2D pose
@@ -348,7 +255,16 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out
    * @param out_pose Output parameter with the robot's current 2D pose
    * @return true if pose was successfully retrieved
    */
-bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
+  bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose);
+
+  /**
+   * @brief Get the robot's current twist
+   * @param handle Navigation handle
+   * @param out_twist Output parameter with the robot's current twist
+   * @return true if twist was successfully retrieved
+   * @note out_twist->header.frame_id must be freed using nav_c_api_free_string
+   */
+  bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist);
 
   /**
    * @brief Get navigation feedback
@@ -357,17 +273,210 @@ bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
    * @return true if feedback was successfully retrieved
    * @note The feed_back_str field must be freed using nav_c_api_free_string
    */
-bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback);
+  bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback);
 
   /**
- * @brief Free navigation feedback structure
+   * @brief Add a static map to the navigation system
- * @param feedback Feedback structure to free
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @param map Occupancy grid handle
+   * @return true if the map was added successfully
    */
-void navigation_free_feedback(NavFeedback* feedback);
+  bool navigation_add_static_map(NavigationHandle handle, const char *map_name, const OccupancyGrid occupancy_grid);
+
+  /**
+   * @brief Add a laser scan to the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @param laser_scan Laser scan handle
+   * @return true if the laser scan was added successfully
+   */
+  bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, const LaserScan laser_scan);
+
+  /**
+   * @brief Add a point cloud to the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @param point_cloud Point cloud handle
+   * @return true if the point cloud was added successfully
+   */
+  bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud);
+
+  /**
+   * @brief Add a point cloud2 to the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @param point_cloud2 Point cloud2 handle
+   * @return true if the point cloud2 was added successfully
+   */
+  bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2);
+
+  /**
+   * @brief Add an odometry to the navigation system
+   * @param handle Navigation handle
+   * @param odometry_name Name of the odometry
+   * @param odometry Odometry handle
+   * @return true if the odometry was added successfully
+   */
+  bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry);
+
+  /**
+   * @brief Get a static map from the navigation system
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @param out_map Output parameter for the map handle
+   * @return true if the map was retrieved successfully
+   */
+  bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map);
+
+  /**
+   * @brief Get a laser scan from the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @param out_scan Output parameter for the laser scan handle
+   * @return true if the laser scan was retrieved successfully
+   */
+  bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan);
+
+  /**
+   * @brief Get a point cloud from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @param out_cloud Output parameter for the point cloud handle
+   * @return true if the point cloud was retrieved successfully
+   */
+  bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud);
+
+  /**
+   * @brief Get a point cloud2 from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @param out_cloud Output parameter for the point cloud2 handle
+   * @return true if the point cloud2 was retrieved successfully
+   */
+  bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud);
+
+  /**
+   * @brief Get all static maps from the navigation system
+   * @param handle Navigation handle
+   * @param out_maps Output parameter for the maps array
+   * @param out_count Output parameter for the number of maps
+   * @return true if the maps were retrieved successfully
+   */
+  bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count);
+
+  /**
+   * @brief Get all laser scans from the navigation system
+   * @param handle Navigation handle
+   * @param out_scans Output parameter for the scans array
+   * @param out_count Output parameter for the number of scans
+   * @return true if the scans were retrieved successfully
+   */
+  bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count);
+
+  /**
+   * @brief Get all point clouds from the navigation system
+   * @param handle Navigation handle
+   * @param out_clouds Output parameter for the clouds array
+   * @param out_count Output parameter for the number of clouds
+   * @return true if the clouds were retrieved successfully
+   */
+  bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count);
+
+  /**
+   * @brief Get all point cloud2s from the navigation system
+   * @param handle Navigation handle
+   * @param out_clouds Output parameter for the clouds array
+   * @param out_count Output parameter for the number of clouds
+   * @return true if the clouds were retrieved successfully
+   */
+  bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count);
+
+  /**
+   * @brief Remove a static map from the navigation system
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @return true if the map was removed successfully
+   */
+  bool navigation_remove_static_map(NavigationHandle handle, const char *map_name);
+
+  /**
+   * @brief Remove a laser scan from the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @return true if the laser scan was removed successfully
+   */
+  bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name);
+
+  /**
+   * @brief Remove a point cloud from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @return true if the point cloud was removed successfully
+   */
+  bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name);
+
+  /**
+   * @brief Remove a point cloud2 from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @return true if the point cloud2 was removed successfully
+   */
+  bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name);
+
+  /**
+   * @brief Remove all static maps from the navigation system
+   * @param handle Navigation handle
+   * @return true if the maps were removed successfully
+   */
+  bool navigation_remove_all_static_maps(NavigationHandle handle);
+
+  /**
+   * @brief Remove all laser scans from the navigation system
+   * @param handle Navigation handle
+   * @return true if the scans were removed successfully
+   */
+  bool navigation_remove_all_laser_scans(NavigationHandle handle);
+
+  /**
+   * @brief Remove all point clouds from the navigation system
+   * @param handle Navigation handle
+   * @return true if the clouds were removed successfully
+   */
+  bool navigation_remove_all_point_clouds(NavigationHandle handle);
+
+  /**
+   * @brief Remove all point cloud2s from the navigation system
+   * @param handle Navigation handle
+   * @return true if the cloud2s were removed successfully
+   */
+  bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
+
+  /**
+   * @brief Remove all data from the navigation system
+   * @param handle Navigation handle
+   * @return true if the data was removed successfully
+   */
+  bool navigation_remove_all_data(NavigationHandle handle);
+
+  /**
+   * @brief Get the global data from the navigation system
+   * @param handle Navigation handle
+   * @param out_data Output parameter for the data
+   * @return true if the data was retrieved successfully
+   */
+  bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data);
+
+  /**
+   * @brief Get the local data from the navigation system
+   * @param handle Navigation handle
+   * @param out_data Output parameter for the data
+   * @return true if the data was retrieved successfully
+   */
+  bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data);
 
 #ifdef __cplusplus
 }
 #endif
 
 #endif // NAVIGATION_C_API_H
-

src/APIs/c_api/include/nav_msgs/ActionTypes.h

@@ -0,0 +1,29 @@
+ #ifndef C_API_NAV_MSGS_ACTIONTYPES_H
+ #define C_API_NAV_MSGS_ACTIONTYPES_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+ #include <stdint.h>
+ #include <stddef.h>
+ #include "std_msgs/Time.h"
+ 
+ typedef struct
+ {
+   Time stamp;
+   char *id;
+ } GoalID;
+ 
+ typedef struct
+ {
+   GoalID goal_id;
+   uint8_t status;
+   char *text;
+ } GoalStatus;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_ACTIONTYPES_H

src/APIs/c_api/include/nav_msgs/GetMap.h

@@ -0,0 +1,23 @@
+ #ifndef C_API_NAV_MSGS_GETMAP_H
+ #define C_API_NAV_MSGS_GETMAP_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_msgs/GetMapRequest.h"
+#include "nav_msgs/GetMapResponse.h"
+
+typedef struct
+{
+  GetMapRequest request;
+  GetMapResponse response;
+} GetMap;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAP_H

src/APIs/c_api/include/nav_msgs/GetMapAction.h

@@ -0,0 +1,25 @@
+ #ifndef C_API_NAV_MSGS_GETMAPACTION_H
+ #define C_API_NAV_MSGS_GETMAPACTION_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_msgs/GetMapActionGoal.h"
+#include "nav_msgs/GetMapActionResult.h"
+#include "nav_msgs/GetMapActionFeedback.h"
+
+typedef struct
+{
+  GetMapActionGoal action_goal;
+  GetMapActionResult action_result;
+  GetMapActionFeedback action_feedback;
+} GetMapAction;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPACTION_H

src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h

@@ -0,0 +1,25 @@
+ #ifndef C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H
+ #define C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_msgs/ActionTypes.h"
+#include "nav_msgs/GetMapFeedback.h"
+
+typedef struct
+{
+  Header header;
+  GoalStatus status;
+  GetMapFeedback feedback;
+} GetMapActionFeedback;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H

src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h

@@ -0,0 +1,25 @@
+ #ifndef C_API_NAV_MSGS_GETMAPACTIONGOAL_H
+ #define C_API_NAV_MSGS_GETMAPACTIONGOAL_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_msgs/ActionTypes.h"
+#include "nav_msgs/GetMapGoal.h"
+
+typedef struct
+{
+  Header header;
+  GoalID goal_id;
+  GetMapGoal goal;
+} GetMapActionGoal;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPACTIONGOAL_H

src/APIs/c_api/include/nav_msgs/GetMapActionResult.h

@@ -0,0 +1,25 @@
+ #ifndef C_API_NAV_MSGS_GETMAPACTIONRESULT_H
+ #define C_API_NAV_MSGS_GETMAPACTIONRESULT_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_msgs/ActionTypes.h"
+#include "nav_msgs/GetMapResult.h"
+
+typedef struct
+{
+  Header header;
+  GoalStatus status;
+  GetMapResult result;
+} GetMapActionResult;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPACTIONRESULT_H

src/APIs/c_api/include/nav_msgs/GetMapFeedback.h

@@ -0,0 +1,20 @@
+ #ifndef C_API_NAV_MSGS_GETMAPFEEDBACK_H
+ #define C_API_NAV_MSGS_GETMAPFEEDBACK_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint8_t _dummy;
+} GetMapFeedback;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPFEEDBACK_H

src/APIs/c_api/include/nav_msgs/GetMapGoal.h

@@ -0,0 +1,20 @@
+ #ifndef C_API_NAV_MSGS_GETMAPGOAL_H
+ #define C_API_NAV_MSGS_GETMAPGOAL_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint8_t _dummy;
+} GetMapGoal;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPGOAL_H

src/APIs/c_api/include/nav_msgs/GetMapRequest.h

@@ -0,0 +1,20 @@
+ #ifndef C_API_NAV_MSGS_GETMAPREQUEST_H
+ #define C_API_NAV_MSGS_GETMAPREQUEST_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint8_t _dummy;
+} GetMapRequest;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPREQUEST_H

src/APIs/c_api/include/nav_msgs/GetMapResponse.h

@@ -0,0 +1,21 @@
+ #ifndef C_API_NAV_MSGS_GETMAPRESPONSE_H
+ #define C_API_NAV_MSGS_GETMAPRESPONSE_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_msgs/OccupancyGrid.h"
+
+typedef struct
+{
+  OccupancyGrid map;
+} GetMapResponse;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETMAPRESPONSE_H