update find path file plugin .so

.gitignore

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

.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,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
@@ -138,6 +138,22 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
+endif()
+
+if (NOT TARGET grid_map_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
+endif()
+
+if (NOT TARGET robot_base_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
+endif()
+
+if (NOT TARGET hybrid_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
+endif()
+
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/config/obstacle_layer_params.yaml

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

config/costmap_common_params.yaml

@@ -1,11 +1,4 @@
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-base_local_planner: LocalPlannerAdapter
-base_global_planner: CustomPlanner
-
-robot_base_frame: base_footprint
+robot_base_frame: base_link
 transform_tolerance: 1.0
 obstacle_range: 3.0
 #mark_threshold: 1
@@ -26,21 +19,37 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
-    topic: /f_scan
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
+  l_scan_marking:
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
-    topic: /f_scan
+  l_scan_clearing:
+    topic: /l_scan
     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
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_marking:
@@ -48,7 +57,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:
@@ -56,118 +65,9 @@ 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
   
 
-global_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
 
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-local_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
-
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25

config/costmap_global_params.yaml

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

config/costmap_local_params.yaml

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

config/dock_global_params.yaml

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

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,59 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017  
+  xy_goal_tolerance: 0.03 
+  min_approach_linear_velocity: 0.06
+
+HybridLocalPlanner:
+# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
+# HybridLocalPlanner:
+    library_path: libhybrid_local_planner
+    odom_topic: odom
+    # Trajectory
+    max_global_plan_lookahead_dist: 4.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_prune_distance: 0.0
+    global_plan_goal_sep: 0.05
+
+    # Robot
+
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.4
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.4
+    robot_max_v_backwards_pt: -0.2
+    acc_lim_x: 1.0
+    acc_lim_theta: 2.0
+    turn_around_priority: True
+    stop_dist: 0.5
+    dec_dist: 1.0
+
+
+    # GoalTolerance
+
+    xy_goal_tolerance: 0.1
+    yaw_goal_tolerance: 0.07
+
+    # Optimization
+
+    # PP Parameters
+    w_vel: 0.8
+    w_omega: 1.0
+    # DWA Parameters
+    enable_backward_motion: false
+    w_targetheading_ac: 1.7
+    w_velocity_ac: 0.2
+    w_clearance_ac: 0.2
+    w_pathDistance_ac: 0.05
+    w_smoothness_ac: 0.3
+    w_targetheading_pt: 0.2
+    w_velocity_pt: 0.8
+    w_clearance_pt: 0.1
+    w_pathDistance_pt: 2.1
+    w_smoothness_pt: 0.3
+    time_horizon: 3.0
+    velocity_resolution: 0.015
+
+    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
+    calibration_factor: 1.5 # Hệ số hiệu chuẩn
+    use_obstacle_avoidance: true # Bật tắt tránh vật cản

config/maker_sources.yaml

@@ -37,7 +37,7 @@ charger:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: charger
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60

config/move_base_common_params.yaml

@@ -1,3 +1,25 @@
+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: 30.0   # run controller at 15.0 Hz

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,4 +1,4 @@
-yaw_goal_tolerance: 0.017  
+yaw_goal_tolerance: 0.03  
 xy_goal_tolerance: 0.02 
 min_approach_linear_velocity: 0.05 
 
@@ -50,13 +50,13 @@ LimitedAccelGenerator:
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
   min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.0
+  acc_lim_x: 1.5
   acc_lim_y: 0.0      # diff drive robot
   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
 
@@ -74,12 +74,19 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.47
   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:
@@ -89,7 +96,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
   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_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
@@ -106,11 +113,12 @@ MKTAlgorithmDiffPredictiveTrajectory:
   final_heading_xy_tolerance: 0.1
   final_heading_angle_tolerance: 0.05
   final_heading_min_velocity: 0.05
-  final_heading_kp_angular: 2.0
+  final_heading_kp_angular: 1.2
+  final_heading_ki_angular: 0.002
+  final_heading_kd_angular: 0.12
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.8
   index_samples: 60
@@ -121,11 +129,11 @@ 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: 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.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) 
+  lookahead_time: 2.0                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 1.0                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 1.0                    # Maximum squared journey to consider for goal (default: 0.2) 
   max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
@@ -142,11 +150,12 @@ MKTAlgorithmDiffGoStraight:
   final_heading_xy_tolerance: 0.1
   final_heading_angle_tolerance: 0.05
   final_heading_min_velocity: 0.05
-  final_heading_kp_angular: 2.0
+  final_heading_kp_angular: 1.5
+  final_heading_ki_angular: 0.2
+  final_heading_kd_angular: 0.05
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.47
   index_samples: 60
@@ -179,6 +188,9 @@ MKTAlgorithmDiffRotateToGoal:
   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

examples/CSharpExample.cs

@@ -1,620 +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 Twist2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Quaternion
-        {
-            public double x;
-            public double y;
-            public double z;
-            public double w;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Position
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose
-        {
-            public Position position;
-            public Quaternion orientation;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct 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 Twist2DStamped
-        {
-            public Header header;
-            public Twist2D velocity;
-        }
-
-        [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;
-        }
-
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NamedOccupancyGrid
-        {
-            public IntPtr name; // char*
-            public IntPtr map;  // OccupancyGridHandle
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NamedLaserScan
-        {
-            public IntPtr name; // char*
-            public IntPtr scan; // LaserScanHandle
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NamedPointCloud
-        {
-            public IntPtr name; // char*
-            public IntPtr cloud; // PointCloudHandle
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NamedPointCloud2
-        {
-            public IntPtr name; // char*
-            public IntPtr cloud; // PointCloud2Handle
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PlannerDataOutput
-        {
-            public IntPtr plan; // Path2DHandle
-            public IntPtr costmap; // OccupancyGridHandle
-            public IntPtr costmap_update; // OccupancyGridUpdateHandle
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_costmap_updated;
-            public IntPtr footprint; // PolygonStampedHandle
-        }
-
-        // ============================================================================
-        // String Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-
-        // ============================================================================
-        // Complex Message Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_occupancy_grid(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_occupancy_grid_update(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_laser_scan(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_point_cloud(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_point_cloud2(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_odometry(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_path2d(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_polygon_stamped(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_order(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
-
-        // ============================================================================
-        // 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_get_robot_footprint(
-            IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_points(IntPtr points);
-
-        [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)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to_order(
-            IntPtr handle, IntPtr order, 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_dock_to_order(
-            IntPtr handle, IntPtr order, 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_twist(
-            IntPtr handle, ref Twist2DStamped out_twist);
-
-        [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);
-
-
-        // ============================================================================
-        // Navigation Data Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_static_map(IntPtr handle, string map_name, IntPtr map);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_static_map(IntPtr handle, string map_name);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_laser_scan(IntPtr 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(IntPtr 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(IntPtr handle, string point_cloud2_name);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_all_static_maps(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_all_laser_scans(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_all_point_clouds(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_remove_all_data(IntPtr handle);
-
-
-        // ============================================================================
-        // Planner Data
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data);
-
-        // ============================================================================
-        // 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);
-            }
-
-
-            // Get global planner data (opaque handles)
-            NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
-            if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
-            {
-                Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})");
-                NavigationAPI.navigation_free_planner_data(ref globalData);
-            }
-
-            // Get all static maps (names + opaque handles)
-            IntPtr mapsPtr;
-            UIntPtr mapsCount;
-            if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount))
-            {
-                ulong count = mapsCount.ToUInt64();
-                Console.WriteLine($"Static maps: {count}");
-                if (mapsPtr != IntPtr.Zero && count > 0)
-                {
-                    int itemSize = Marshal.SizeOf<NavigationAPI.NamedOccupancyGrid>();
-                    for (ulong i = 0; i < count; i++)
-                    {
-                        IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize)));
-                        var item = Marshal.PtrToStructure<NavigationAPI.NamedOccupancyGrid>(itemPtr);
-                        string name = NavigationAPI.MarshalString(item.name);
-                        Console.WriteLine($"- {name}");
-                    }
-                    NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count);
-                }
-            }
-
-            // 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

@@ -2,9 +2,11 @@
   <PropertyGroup>
     <OutputType>Exe</OutputType>
     <TargetFramework>net6.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/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
-echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# # Bước 3: Copy library
+# echo "Copying library..."
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 # Main build directory (contains libtf3.so, librobot_cpp.so, etc.)

pnkx_nav_core.sln

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

src/APIs/c_api/CMakeLists.txt

@@ -10,17 +10,20 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-# Find Boost
-find_package(Boost REQUIRED)
+# Find Boost (filesystem needed for plugin path / boost::dll usage)
+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 HEADERS "include/*.h")
+file(GLOB SOURCES 
+    "src/*.cpp" 
+    "src/std_msgs/*.cpp" 
+    "src/sensor_msgs/*.cpp" 
+    "src/geometry_msgs/*.cpp"
+    "src/nav_msgs/*.cpp"
+    "src/nav_2d_msgs/*.cpp"
+    "src/robot_nav_msgs/*.cpp"
+    "src/robot_geometry_msgs/*.cpp"
+    "src/map_msgs/*.cpp"
+)
+file(GLOB HEADERS 
+    "include/*.h" 
+    "include/std_msgs/*.h" 
+    "include/sensor_msgs/*.h" 
+    "include/geometry_msgs/*.h"
+    "include/nav_msgs/*.h"
+    "include/nav_2d_msgs/*.h"
+    "include/robot_nav_msgs/*.h"
+    "include/robot_geometry_msgs/*.h"
+    "include/map_msgs/*.h"
+)
 
 # Tạo thư viện shared (.so)
 add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
@@ -41,6 +64,8 @@ target_link_libraries(nav_c_api
         ${PACKAGES_DIR}
     PRIVATE
         Boost::boost
+        Boost::filesystem
+        Boost::system
         dl
 )
 

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

@@ -9,151 +9,48 @@ extern "C"
   #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 void *OccupancyGridHandle;
-  typedef void *OccupancyGridUpdateHandle;
-  typedef void *LaserScanHandle;
-  typedef void *PointCloudHandle;
-  typedef void *PointCloud2Handle;
-  typedef void *OdometryHandle;
-  typedef void *Path2DHandle;
-  typedef void *PolygonStampedHandle;
-  typedef void *OrderHandle;
   
-  // ============================================================================
-  // Enums
-  // ============================================================================
-
-  /**
-   * @brief Navigation states, including planning and controller status
-   */
   typedef enum
   {
-    NAV_STATE_PENDING = 0,
-    NAV_STATE_ACTIVE = 1,
-    NAV_STATE_PREEMPTED = 2,
-    NAV_STATE_SUCCEEDED = 3,
-    NAV_STATE_ABORTED = 4,
-    NAV_STATE_REJECTED = 5,
-    NAV_STATE_PREEMPTING = 6,
-    NAV_STATE_RECALLING = 7,
-    NAV_STATE_RECALLED = 8,
-    NAV_STATE_LOST = 9,
-    NAV_STATE_PLANNING = 10,
-    NAV_STATE_CONTROLLING = 11,
-    NAV_STATE_CLEARING = 12,
-    NAV_STATE_PAUSED = 13
+    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
   } NavigationState;
   
-  // ============================================================================
-  // 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 Twist2D structure (x, y, theta velocities)
-   */
-  typedef struct
-  {
-    double x;
-    double y;
-    double theta;
-  } Twist2D;
-
-  /**
-   * @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 Twist2DStamped structure
-   */
-  typedef struct
-  {
-    Header header;
-    Twist2D velocity;
-  } Twist2DStamped;
-
-  /**
-   * @brief Vector3 structure
-   */
-  typedef struct
-  {
-    double x;
-    double y;
-    double z;
-  } Vector3;
-
-  /**
-   * @brief Navigation feedback structure
-   */
   typedef struct
   {
     NavigationState navigation_state;
@@ -164,201 +61,24 @@ extern "C"
   } NavFeedback;
 
   /**
-   * @brief Named OccupancyGrid structure
-   * @note map is an opaque handle to a C++ robot_nav_msgs::OccupancyGrid
+   * @brief Planner data output structure (C version).
+   * Mirrors robot::move_base_core::PlannerDataOutput in `move_base_core/navigation.h`.
    */
   typedef struct
   {
-    char *name;
-    OccupancyGridHandle map;
-  } NamedOccupancyGrid;
-
-  /**
-   * @brief Named LaserScan structure
-   * @note scan is an opaque handle to a C++ robot_sensor_msgs::LaserScan
-   */
-  typedef struct
-  {
-    char *name;
-    LaserScanHandle scan;
-  } NamedLaserScan;
-
-  /**
-   * @brief Named PointCloud structure
-   * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud
-   */
-  typedef struct
-  {
-    char *name;
-    PointCloudHandle cloud;
-  } NamedPointCloud;
-
-  /**
-   * @brief Named PointCloud2 structure
-   * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud2
-   */
-  typedef struct
-  {
-    char *name;
-    PointCloud2Handle cloud;
-  } NamedPointCloud2;
-
-  /**
-   * @brief Planner data output structure (opaque message handles)
-   */
-  typedef struct
-  {
-    Path2DHandle plan;
-    OccupancyGridHandle costmap;
-    OccupancyGridUpdateHandle costmap_update;
+    Path2D plan;
+    OccupancyGrid costmap;
+    OccupancyGridUpdate costmap_update;
     bool is_costmap_updated;
-    PolygonStampedHandle footprint;
+    PolygonStamped footprint;
   } PlannerDataOutput;
 
-  // ============================================================================
-  // String Management
-  // ============================================================================
-
   /**
-   * @brief Free a string allocated by the library
-   * @param str String to free
+   * @brief Free a string allocated by the API (e.g. feed_back_str, navigation_state_to_string result).
+   * @param str Pointer from strdup; no-op if NULL.
    */
   void nav_c_api_free_string(char *str);
 
-  // ============================================================================
-  // Complex Message Handle Management
-  // ============================================================================
-
-  /**
-   * @brief Free an occupancy grid handle
-   * @param handle Occupancy grid handle to free
-   */
-  void navigation_free_occupancy_grid(OccupancyGridHandle handle);
-  /**
-   * @brief Free an occupancy grid update handle
-   * @param handle Occupancy grid update handle to free
-   */
-  void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle);
-
-  /**
-   * @brief Free a laser scan handle
-   * @param handle Laser scan handle to free
-   */
-  void navigation_free_laser_scan(LaserScanHandle handle);
-
-  /**
-   * @brief Free a point cloud handle
-   * @param handle Point cloud handle to free
-   */
-  void navigation_free_point_cloud(PointCloudHandle handle);
-
-  /**
-   * @brief Free a point cloud2 handle
-   * @param handle Point cloud2 handle to free
-   */
-  void navigation_free_point_cloud2(PointCloud2Handle handle);
-
-  /**
-   * @brief Free an odometry handle
-   * @param handle Odometry handle to free
-   */
-  void navigation_free_odometry(OdometryHandle handle);
-
-  /**
-   * @brief Free a path2d handle
-   * @param handle Path2d handle to free
-   */
-  void navigation_free_path2d(Path2DHandle handle);
-
-  /**
-   * @brief Free a polygon stamped handle
-   * @param handle Polygon stamped handle to free
-   */
-  void navigation_free_polygon_stamped(PolygonStampedHandle handle);
-
-  /**
-   * @brief Free an order handle
-   * @param handle Order handle to free
-   */
-  void navigation_free_order(OrderHandle handle);
-
-  /**
-   * @brief Free an array of named occupancy grids
-   * @param maps Array of named occupancy grids to free
-   * @param count Number of named occupancy grids in the array
-   */
-  void navigation_free_named_occupancy_grids(NamedOccupancyGrid *maps, size_t count);
-
-  /**
-   * @brief Free an array of named laser scans
-   * @param scans Array of named laser scans to free
-   * @param count Number of named laser scans in the array
-   */
-  void navigation_free_named_laser_scans(NamedLaserScan *scans, size_t count);
-
-  /**
-   * @brief Free an array of named point clouds
-   * @param clouds Array of named point clouds to free
-   * @param count Number of named point clouds in the array
-   */
-  void navigation_free_named_point_clouds(NamedPointCloud *clouds, size_t count);
-
-  /**
-   * @brief Free an array of named point cloud2s
-   * @param clouds Array of named point cloud2s to free
-   * @param count Number of named point cloud2s in the array
-   */
-  void navigation_free_named_point_cloud2s(NamedPointCloud2 *clouds, size_t count);
-
-  /**
-   * @brief Free a planner data output
-   * @param data Planner data output to free
-   */
-  void navigation_free_planner_data(PlannerDataOutput *data);
-
-  // ============================================================================
-  // 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
@@ -371,57 +91,18 @@ extern "C"
    */
   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)
@@ -439,83 +120,86 @@ extern "C"
    * @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);
+  bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count);
 
-  /**
-   * @brief Free a points array allocated by navigation_get_robot_footprint
-   * @param points Pointer to point array
-   */
-  void navigation_free_points(Point *points);
 
   /**
    * @brief Send a goal for the robot to navigate to
    * @param handle Navigation handle
    * @param goal Target pose in the global frame
-   * @param xy_goal_tolerance Acceptable error in X/Y (meters)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if goal was accepted and sent successfully
    */
-  bool navigation_move_to(NavigationHandle handle, const PoseStamped *goal,
-                          double xy_goal_tolerance, double yaw_goal_tolerance);
+  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal);
 
   /**
    * @brief Send a goal for the robot to navigate to
    * @param handle Navigation handle
    * @param order Order message
    * @param goal Target pose in the global frame
-   * @param xy_goal_tolerance Acceptable error in X/Y (meters)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
+   * @return true if goal was accepted and sent successfully
+   * @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_order(NavigationHandle handle, const OrderHandle order,
-                                const PoseStamped *goal,
-                                double xy_goal_tolerance, double yaw_goal_tolerance);
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param marker Marker name or ID (null-terminated string)
    * @param goal Target pose for docking
-   * @param xy_goal_tolerance Acceptable XY error (meters)
-   * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-  bool navigation_dock_to(NavigationHandle handle, const char *marker,
-                          const PoseStamped *goal,
-                          double xy_goal_tolerance, double yaw_goal_tolerance);
+  bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal);
 
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param order Order message
    * @param goal Target pose for docking
-   * @param xy_goal_tolerance Acceptable XY error (meters)
-   * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-  bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
-                                const PoseStamped *goal,
-                                double xy_goal_tolerance, double yaw_goal_tolerance);
+  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 xy_goal_tolerance Acceptable positional error (meters)
+   * @param distance Distance to move (meters)
    * @return true if command issued successfully
    */
-  bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped *goal,
-                                   double xy_goal_tolerance);
+  bool navigation_move_straight_to(NavigationHandle handle, const double distance);
 
   /**
    * @brief Rotate in place to align with target orientation
    * @param handle Navigation handle
-   * @param goal Pose containing desired heading (only Z-axis used)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
+   * @param goal_yaw Desired heading (radians)
    * @return true if rotation command was sent successfully
    */
-  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped *goal,
-                            double yaw_goal_tolerance);
+  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal);
 
   /**
    * @brief Pause the robot's movement
@@ -563,7 +247,7 @@ extern "C"
    * @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
@@ -571,7 +255,7 @@ extern "C"
    * @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
@@ -580,7 +264,7 @@ extern "C"
    * @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);
+  bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist);
 
   /**
    * @brief Get navigation feedback
@@ -589,13 +273,7 @@ extern "C"
    * @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);
-
-  /**
-   * @brief Free navigation feedback structure
-   * @param feedback Feedback structure to free
-   */
-  void navigation_free_feedback(NavFeedback *feedback);
+  bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback);
 
   /**
    * @brief Add a static map to the navigation system
@@ -604,7 +282,7 @@ extern "C"
    * @param map Occupancy grid handle
    * @return true if the map was added successfully
    */
-  bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle map);
+  bool navigation_add_static_map(NavigationHandle handle, const char *map_name, const OccupancyGrid occupancy_grid);
 
   /**
    * @brief Add a laser scan to the navigation system
@@ -613,7 +291,7 @@ extern "C"
    * @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, LaserScanHandle laser_scan);
+  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
@@ -622,7 +300,7 @@ extern "C"
    * @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, PointCloudHandle point_cloud);
+  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
@@ -631,7 +309,7 @@ extern "C"
    * @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, PointCloud2Handle point_cloud2);
+  bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2);
 
   /**
    * @brief Add an odometry to the navigation system
@@ -640,7 +318,7 @@ extern "C"
    * @param odometry Odometry handle
    * @return true if the odometry was added successfully
    */
-  bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, OdometryHandle odometry);
+  bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry);
 
   /**
    * @brief Get a static map from the navigation system
@@ -649,7 +327,7 @@ extern "C"
    * @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, OccupancyGridHandle *out_map);
+  bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map);
 
   /**
    * @brief Get a laser scan from the navigation system
@@ -658,7 +336,7 @@ extern "C"
    * @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, LaserScanHandle *out_scan);
+  bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan);
 
   /**
    * @brief Get a point cloud from the navigation system
@@ -667,7 +345,7 @@ extern "C"
    * @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, PointCloudHandle *out_cloud);
+  bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud);
 
   /**
    * @brief Get a point cloud2 from the navigation system
@@ -676,7 +354,7 @@ extern "C"
    * @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, PointCloud2Handle *out_cloud);
+  bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud);
 
   /**
    * @brief Get all static maps from the navigation system
@@ -685,7 +363,7 @@ extern "C"
    * @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);
+  bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count);
 
   /**
    * @brief Get all laser scans from the navigation system
@@ -694,7 +372,7 @@ extern "C"
    * @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);
+  bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count);
 
   /**
    * @brief Get all point clouds from the navigation system
@@ -703,7 +381,7 @@ extern "C"
    * @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);
+  bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count);
 
   /**
    * @brief Get all point cloud2s from the navigation system
@@ -712,7 +390,7 @@ extern "C"
    * @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);
+  bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count);
 
   /**
    * @brief Remove a static map from the navigation system

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

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

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

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

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

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

@@ -0,0 +1,23 @@
+ #ifndef C_API_NAV_MSGS_GETPLANREQUEST_H
+ #define C_API_NAV_MSGS_GETPLANREQUEST_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/PoseStamped.h"
+
+typedef struct
+{
+  PoseStamped start;
+  PoseStamped goal;
+  float tolerance;
+} GetPlanRequest;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETPLANREQUEST_H

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

@@ -0,0 +1,21 @@
+ #ifndef C_API_NAV_MSGS_GETPLANRESPONSE_H
+ #define C_API_NAV_MSGS_GETPLANRESPONSE_H
+ 
+ #ifdef __cplusplus
+ extern "C" {
+ #endif
+ 
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_msgs/Path.h"
+
+typedef struct
+{
+  Path plan;
+} GetPlanResponse;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GETPLANRESPONSE_H

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

@@ -0,0 +1,26 @@
+ #ifndef C_API_NAV_MSGS_GRIDCELLS_H
+ #define C_API_NAV_MSGS_GRIDCELLS_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;
+  float cell_width;
+  float cell_height;
+  Point *cells;
+  size_t cells_count;
+} GridCells;
+ 
+ #ifdef __cplusplus
+ }
+ #endif
+ 
+ #endif // C_API_NAV_MSGS_GRIDCELLS_H

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

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

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

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