update c_api

examples/CSharpExample.cs

@@ -128,12 +128,98 @@ namespace NavigationExample
             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
         // ============================================================================
@@ -209,12 +295,24 @@ namespace NavigationExample
             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(
@@ -267,6 +365,118 @@ namespace NavigationExample
         [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);
+
+
+        // ============================================================================
+        // Navigation Commands with Order
+        // ============================================================================
+
+
+
+
         // ============================================================================
         // Helper Methods for String Conversion
         // ============================================================================
@@ -379,6 +589,36 @@ namespace NavigationExample
                 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/Program.cs

@@ -128,12 +128,98 @@ namespace NavigationExample
             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
         // ============================================================================
@@ -209,12 +295,24 @@ namespace NavigationExample
             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(
@@ -267,6 +365,118 @@ namespace NavigationExample
         [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);
+
+
+        // ============================================================================
+        // Navigation Commands with Order
+        // ============================================================================
+
+
+
+
         // ============================================================================
         // Helper Methods for String Conversion
         // ============================================================================
@@ -379,6 +589,36 @@ namespace NavigationExample
                 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);

src/APIs/c_api/include/nav_c_api.h

@@ -2,25 +2,36 @@
 #define NAVIGATION_C_API_H
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
 
 #include <stdbool.h>
 #include <stdint.h>
 #include <stddef.h>
 
-// Forward declarations
+  // Forward declarations
-typedef void* NavigationHandle;
+  typedef void *NavigationHandle;
-typedef void* TFListenerHandle;
+  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
+  // Enums
-// ============================================================================
+  // ============================================================================
 
-/**
+  /**
    * @brief Navigation states, including planning and controller status
    */
-typedef enum {
+  typedef enum
+  {
     NAV_STATE_PENDING = 0,
     NAV_STATE_ACTIVE = 1,
     NAV_STATE_PREEMPTED = 2,
@@ -35,138 +46,292 @@ typedef enum {
     NAV_STATE_CONTROLLING = 11,
     NAV_STATE_CLEARING = 12,
     NAV_STATE_PAUSED = 13
-} NavigationState;
+  } NavigationState;
 
-// ============================================================================
+  // ============================================================================
-// Structures
+  // Structures
-// ============================================================================
+  // ============================================================================
 
-/**
+  /**
    * @brief Point structure (x, y, z)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
-} Point;
+  } Point;
 
-/**
+  /**
    * @brief Pose2D structure (x, y, theta)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double theta;
-} Pose2D;
+  } Pose2D;
 
-/**
+  /**
    * @brief Twist2D structure (x, y, theta velocities)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double theta;
-} Twist2D;
+  } Twist2D;
 
-/**
+  /**
    * @brief Quaternion structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
     double w;
-} Quaternion;
+  } Quaternion;
 
-/**
+  /**
    * @brief Position structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
-} Position;
+  } Position;
 
-/**
+  /**
    * @brief Pose structure
    */
-typedef struct {
+  typedef struct
+  {
     Position position;
     Quaternion orientation;
-} Pose;
+  } Pose;
 
-/**
+  /**
    * @brief Header structure
    */
-typedef struct {
+  typedef struct
+  {
     uint32_t seq;
     int64_t sec;
     uint32_t nsec;
-    char* frame_id;
+    char *frame_id;
-} Header;
+  } Header;
 
-/**
+  /**
    * @brief PoseStamped structure
    */
-typedef struct {
+  typedef struct
+  {
     Header header;
     Pose pose;
-} PoseStamped;
+  } PoseStamped;
 
-/**
+  /**
    * @brief Twist2DStamped structure
    */
-typedef struct {
+  typedef struct
+  {
     Header header;
     Twist2D velocity;
-} Twist2DStamped;
+  } Twist2DStamped;
 
-/**
+  /**
    * @brief Vector3 structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
-} Vector3;
+  } Vector3;
 
-/**
+  /**
    * @brief Navigation feedback structure
    */
-typedef struct {
+  typedef struct
+  {
     NavigationState navigation_state;
-    char* feed_back_str;
+    char *feed_back_str;
     Pose2D current_pose;
     bool goal_checked;
     bool is_ready;
-} NavFeedback;
+  } NavFeedback;
 
-// ============================================================================
+  /**
-// String Management
+   * @brief Named OccupancyGrid structure
-// ============================================================================
+   * @note map is an opaque handle to a C++ robot_nav_msgs::OccupancyGrid
+   */
+  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;
+    bool is_costmap_updated;
+    PolygonStampedHandle footprint;
+  } PlannerDataOutput;
+
+  // ============================================================================
+  // String Management
+  // ============================================================================
+
+  /**
    * @brief Free a string allocated by the library
    * @param str String to free
    */
-void nav_c_api_free_string(char* str);
+  void nav_c_api_free_string(char *str);
 
-// ============================================================================
+  // ============================================================================
-// State Conversion
+  // 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);
+  char *navigation_state_to_string(NavigationState state);
 
-// ============================================================================
+  // ============================================================================
-// Helper Functions
+  // 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
@@ -176,53 +341,53 @@ char* navigation_state_to_string(NavigationState state);
    * @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,
+  bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
-                               const char* frame_id, double offset_distance,
+                                 const char *frame_id, double offset_distance,
-                               PoseStamped* out_goal);
+                                 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,
+  bool navigation_offset_goal_stamped(const PoseStamped *in_pose, double offset_distance,
-                                     PoseStamped* out_goal);
+                                      PoseStamped *out_goal);
 
-// ============================================================================
+  // ============================================================================
-// Navigation Handle Management
+  // Navigation Handle Management
-// ============================================================================
+  // ============================================================================
 
-/**
+  /**
    * @brief Create a new navigation instance
    * @return Navigation handle, or NULL on failure
    */
-NavigationHandle navigation_create(void);
+  NavigationHandle navigation_create(void);
 
-/**
+  /**
    * @brief Destroy a navigation instance
    * @param handle Navigation handle to destroy
    */
-void navigation_destroy(NavigationHandle handle);
+  void navigation_destroy(NavigationHandle handle);
 
-// ============================================================================
+  // ============================================================================
-// TF Listener Management
+  // TF Listener Management
-// ============================================================================
+  // ============================================================================
 
-/**
+  /**
    * @brief Create a TF listener instance
    * @return TF listener handle, or NULL on failure
    */
-TFListenerHandle tf_listener_create(void);
+  TFListenerHandle tf_listener_create(void);
 
-/**
+  /**
    * @brief Destroy a TF listener instance
    * @param handle TF listener handle to destroy
    */
-void tf_listener_destroy(TFListenerHandle handle);
+  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.
@@ -240,49 +405,49 @@ void tf_listener_destroy(TFListenerHandle handle);
    * @param qw Rotation quaternion w
    * @return true on success, false on failure
    */
-bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
+  bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
-                                     const char* parent_frame,
+                                        const char *parent_frame,
-                                     const char* child_frame,
+                                        const char *child_frame,
                                         double x, double y, double z,
                                         double qx, double qy, double qz, double qw);
 
-// ============================================================================
+  // ============================================================================
-// Navigation Interface Methods
+  // Navigation Interface Methods
-// ============================================================================
+  // ============================================================================
 
-/**
+  /**
    * @brief Initialize the navigation system
    * @param handle Navigation handle
    * @param tf_handle TF listener handle
    * @return true on success, false on failure
    */
-bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
 
-/**
+  /**
    * @brief Set the robot's footprint (outline shape)
    * @param handle Navigation handle
    * @param points Array of points representing the footprint polygon
    * @param point_count Number of points in the array
    * @return true on success, false on failure
    */
-bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
+  bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count);
 
-/**
+  /**
    * @brief Get the robot's footprint (outline shape)
    * @param handle Navigation handle
    * @param out_points Output array of points (allocated by library, free with navigation_free_points)
    * @param out_count Output number of points in the array
    * @return true on success, false on failure
    */
-bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count);
+  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);
+  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
@@ -290,10 +455,23 @@ void navigation_free_points(Point* points);
    * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if goal was accepted and sent successfully
    */
-bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_move_to(NavigationHandle handle, const PoseStamped *goal,
                           double xy_goal_tolerance, double yaw_goal_tolerance);
 
-/**
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose in the global frame
+   * @param xy_goal_tolerance Acceptable error in X/Y (meters)
+   * @param yaw_goal_tolerance Acceptable angular error (radians)
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order,
+                                const PoseStamped *goal,
+                                double xy_goal_tolerance, double yaw_goal_tolerance);
+
+  /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param marker Marker name or ID (null-terminated string)
@@ -302,49 +480,62 @@ bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
    * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-bool navigation_dock_to(NavigationHandle handle, const char* marker,
+  bool navigation_dock_to(NavigationHandle handle, const char *marker,
-                        const PoseStamped* goal,
+                          const PoseStamped *goal,
                           double xy_goal_tolerance, double yaw_goal_tolerance);
 
-/**
+  /**
+   * @brief Send a docking goal to a predefined marker
+   * @param handle Navigation handle
+   * @param order Order message
+   * @param goal Target pose for docking
+   * @param xy_goal_tolerance Acceptable XY error (meters)
+   * @param yaw_goal_tolerance Acceptable heading error (radians)
+   * @return true if docking command succeeded
+   */
+  bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
+                                const PoseStamped *goal,
+                                double xy_goal_tolerance, double yaw_goal_tolerance);
+
+  /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle
    * @param goal Target pose
    * @param xy_goal_tolerance Acceptable positional error (meters)
    * @return true if command issued successfully
    */
-bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped *goal,
                                    double xy_goal_tolerance);
 
-/**
+  /**
    * @brief Rotate in place to align with target orientation
    * @param handle Navigation handle
    * @param goal Pose containing desired heading (only Z-axis used)
    * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if rotation command was sent successfully
    */
-bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+  bool navigation_rotate_to(NavigationHandle handle, const PoseStamped *goal,
                             double yaw_goal_tolerance);
 
-/**
+  /**
    * @brief Pause the robot's movement
    * @param handle Navigation handle
    */
-void navigation_pause(NavigationHandle handle);
+  void navigation_pause(NavigationHandle handle);
 
-/**
+  /**
    * @brief Resume motion after a pause
    * @param handle Navigation handle
    */
-void navigation_resume(NavigationHandle handle);
+  void navigation_resume(NavigationHandle handle);
 
-/**
+  /**
    * @brief Cancel the current goal and stop the robot
    * @param handle Navigation handle
    */
-void navigation_cancel(NavigationHandle handle);
+  void navigation_cancel(NavigationHandle handle);
 
-/**
+  /**
    * @brief Send limited linear velocity command
    * @param handle Navigation handle
    * @param linear_x Linear velocity in X direction
@@ -352,10 +543,10 @@ void navigation_cancel(NavigationHandle handle);
    * @param linear_z Linear velocity in Z direction
    * @return true if the command was accepted
    */
-bool navigation_set_twist_linear(NavigationHandle handle,
+  bool navigation_set_twist_linear(NavigationHandle handle,
                                    double linear_x, double linear_y, double linear_z);
 
-/**
+  /**
    * @brief Send limited angular velocity command
    * @param handle Navigation handle
    * @param angular_x Angular velocity around X axis
@@ -363,52 +554,251 @@ bool navigation_set_twist_linear(NavigationHandle handle,
    * @param angular_z Angular velocity around Z axis
    * @return true if the command was accepted
    */
-bool navigation_set_twist_angular(NavigationHandle handle,
+  bool navigation_set_twist_angular(NavigationHandle handle,
                                     double angular_x, double angular_y, double angular_z);
 
-/**
+  /**
    * @brief Get the robot's pose as a PoseStamped
    * @param handle Navigation handle
    * @param out_pose Output parameter with the robot's current pose
    * @return true if pose was successfully retrieved
    */
-bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose);
+  bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped *out_pose);
 
-/**
+  /**
    * @brief Get the robot's pose as a 2D pose
    * @param handle Navigation handle
    * @param out_pose Output parameter with the robot's current 2D pose
    * @return true if pose was successfully retrieved
    */
-bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
+  bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D *out_pose);
 
-/**
+  /**
    * @brief Get the robot's current twist
    * @param handle Navigation handle
    * @param out_twist Output parameter with the robot's current twist
    * @return true if twist was successfully retrieved
    * @note out_twist->header.frame_id must be freed using nav_c_api_free_string
    */
-bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist);
+  bool navigation_get_twist(NavigationHandle handle, Twist2DStamped *out_twist);
 
-/**
+  /**
    * @brief Get navigation feedback
    * @param handle Navigation handle
    * @param out_feedback Output parameter with navigation feedback
    * @return true if feedback was successfully retrieved
    * @note The feed_back_str field must be freed using nav_c_api_free_string
    */
-bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback);
+  bool navigation_get_feedback(NavigationHandle handle, NavFeedback *out_feedback);
 
-/**
+  /**
    * @brief Free navigation feedback structure
    * @param feedback Feedback structure to free
    */
-void navigation_free_feedback(NavFeedback* feedback);
+  void navigation_free_feedback(NavFeedback *feedback);
+
+  /**
+   * @brief Add a static map to the navigation system
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @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);
+
+  /**
+   * @brief Add a laser scan to the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @param laser_scan Laser scan handle
+   * @return true if the laser scan was added successfully
+   */
+  bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle laser_scan);
+
+  /**
+   * @brief Add a point cloud to the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @param point_cloud Point cloud handle
+   * @return true if the point cloud was added successfully
+   */
+  bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle point_cloud);
+
+  /**
+   * @brief Add a point cloud2 to the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @param point_cloud2 Point cloud2 handle
+   * @return true if the point cloud2 was added successfully
+   */
+  bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle point_cloud2);
+
+  /**
+   * @brief Add an odometry to the navigation system
+   * @param handle Navigation handle
+   * @param odometry_name Name of the odometry
+   * @param odometry Odometry handle
+   * @return true if the odometry was added successfully
+   */
+  bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, OdometryHandle odometry);
+
+  /**
+   * @brief Get a static map from the navigation system
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @param out_map Output parameter for the map handle
+   * @return true if the map was retrieved successfully
+   */
+  bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle *out_map);
+
+  /**
+   * @brief Get a laser scan from the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @param out_scan Output parameter for the laser scan handle
+   * @return true if the laser scan was retrieved successfully
+   */
+  bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle *out_scan);
+
+  /**
+   * @brief Get a point cloud from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @param out_cloud Output parameter for the point cloud handle
+   * @return true if the point cloud was retrieved successfully
+   */
+  bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle *out_cloud);
+
+  /**
+   * @brief Get a point cloud2 from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @param out_cloud Output parameter for the point cloud2 handle
+   * @return true if the point cloud2 was retrieved successfully
+   */
+  bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle *out_cloud);
+
+  /**
+   * @brief Get all static maps from the navigation system
+   * @param handle Navigation handle
+   * @param out_maps Output parameter for the maps array
+   * @param out_count Output parameter for the number of maps
+   * @return true if the maps were retrieved successfully
+   */
+  bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid **out_maps, size_t *out_count);
+
+  /**
+   * @brief Get all laser scans from the navigation system
+   * @param handle Navigation handle
+   * @param out_scans Output parameter for the scans array
+   * @param out_count Output parameter for the number of scans
+   * @return true if the scans were retrieved successfully
+   */
+  bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan **out_scans, size_t *out_count);
+
+  /**
+   * @brief Get all point clouds from the navigation system
+   * @param handle Navigation handle
+   * @param out_clouds Output parameter for the clouds array
+   * @param out_count Output parameter for the number of clouds
+   * @return true if the clouds were retrieved successfully
+   */
+  bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud **out_clouds, size_t *out_count);
+
+  /**
+   * @brief Get all point cloud2s from the navigation system
+   * @param handle Navigation handle
+   * @param out_clouds Output parameter for the clouds array
+   * @param out_count Output parameter for the number of clouds
+   * @return true if the clouds were retrieved successfully
+   */
+  bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 **out_clouds, size_t *out_count);
+
+  /**
+   * @brief Remove a static map from the navigation system
+   * @param handle Navigation handle
+   * @param map_name Name of the map
+   * @return true if the map was removed successfully
+   */
+  bool navigation_remove_static_map(NavigationHandle handle, const char *map_name);
+
+  /**
+   * @brief Remove a laser scan from the navigation system
+   * @param handle Navigation handle
+   * @param laser_scan_name Name of the laser scan
+   * @return true if the laser scan was removed successfully
+   */
+  bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name);
+
+  /**
+   * @brief Remove a point cloud from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud_name Name of the point cloud
+   * @return true if the point cloud was removed successfully
+   */
+  bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name);
+
+  /**
+   * @brief Remove a point cloud2 from the navigation system
+   * @param handle Navigation handle
+   * @param point_cloud2_name Name of the point cloud2
+   * @return true if the point cloud2 was removed successfully
+   */
+  bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name);
+
+  /**
+   * @brief Remove all static maps from the navigation system
+   * @param handle Navigation handle
+   * @return true if the maps were removed successfully
+   */
+  bool navigation_remove_all_static_maps(NavigationHandle handle);
+
+  /**
+   * @brief Remove all laser scans from the navigation system
+   * @param handle Navigation handle
+   * @return true if the scans were removed successfully
+   */
+  bool navigation_remove_all_laser_scans(NavigationHandle handle);
+
+  /**
+   * @brief Remove all point clouds from the navigation system
+   * @param handle Navigation handle
+   * @return true if the clouds were removed successfully
+   */
+  bool navigation_remove_all_point_clouds(NavigationHandle handle);
+
+  /**
+   * @brief Remove all point cloud2s from the navigation system
+   * @param handle Navigation handle
+   * @return true if the cloud2s were removed successfully
+   */
+  bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
+
+  /**
+   * @brief Remove all data from the navigation system
+   * @param handle Navigation handle
+   * @return true if the data was removed successfully
+   */
+  bool navigation_remove_all_data(NavigationHandle handle);
+
+  /**
+   * @brief Get the global data from the navigation system
+   * @param handle Navigation handle
+   * @param out_data Output parameter for the data
+   * @return true if the data was retrieved successfully
+   */
+  bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data);
+
+  /**
+   * @brief Get the local data from the navigation system
+   * @param handle Navigation handle
+   * @param out_data Output parameter for the data
+   * @return true if the data was retrieved successfully
+   */
+  bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data);
 
 #ifdef __cplusplus
 }
 #endif
 
 #endif // NAVIGATION_C_API_H
-

src/APIs/c_api/src/nav_c_api.cpp

@@ -6,14 +6,24 @@
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
 #include <robot_std_msgs/Header.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_nav_msgs/Odometry.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_protocol_msgs/Order.h>
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 #include <tf3/buffer_core.h>
 #include <string>
 #include <vector>
 #include <cstring>
 #include <cstdlib>
 #include <stdexcept>
+#include <new>
 #include <robot/robot.h>
 
 
@@ -100,6 +110,26 @@ namespace {
         c_feedback->goal_checked = cpp_feedback.goal_checked;
         c_feedback->is_ready = cpp_feedback.is_ready;
     }
+
+    template <typename T>
+    T* clone_message(const T& src) {
+        return new (std::nothrow) T(src);
+    }
+
+    void clear_planner_data(PlannerDataOutput* data) {
+        if (!data) {
+            return;
+        }
+        delete static_cast<robot_nav_2d_msgs::Path2D*>(data->plan);
+        delete static_cast<robot_nav_msgs::OccupancyGrid*>(data->costmap);
+        delete static_cast<robot_map_msgs::OccupancyGridUpdate*>(data->costmap_update);
+        delete static_cast<robot_geometry_msgs::PolygonStamped*>(data->footprint);
+        data->plan = nullptr;
+        data->costmap = nullptr;
+        data->costmap_update = nullptr;
+        data->footprint = nullptr;
+        data->is_costmap_updated = false;
+    }
 }
 
 // ============================================================================
@@ -533,3 +563,583 @@ extern "C" void navigation_free_feedback(NavFeedback* feedback) {
     }
 }
 
+// ============================================================================
+// Complex Message Handle Management
+// ============================================================================
+
+extern "C" void navigation_free_occupancy_grid(OccupancyGridHandle handle) {
+    delete static_cast<robot_nav_msgs::OccupancyGrid*>(handle);
+}
+
+extern "C" void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle) {
+    delete static_cast<robot_map_msgs::OccupancyGridUpdate*>(handle);
+}
+
+extern "C" void navigation_free_laser_scan(LaserScanHandle handle) {
+    delete static_cast<robot_sensor_msgs::LaserScan*>(handle);
+}
+
+extern "C" void navigation_free_point_cloud(PointCloudHandle handle) {
+    delete static_cast<robot_sensor_msgs::PointCloud*>(handle);
+}
+
+extern "C" void navigation_free_point_cloud2(PointCloud2Handle handle) {
+    delete static_cast<robot_sensor_msgs::PointCloud2*>(handle);
+}
+
+extern "C" void navigation_free_odometry(OdometryHandle handle) {
+    delete static_cast<robot_nav_msgs::Odometry*>(handle);
+}
+
+extern "C" void navigation_free_path2d(Path2DHandle handle) {
+    delete static_cast<robot_nav_2d_msgs::Path2D*>(handle);
+}
+
+extern "C" void navigation_free_polygon_stamped(PolygonStampedHandle handle) {
+    delete static_cast<robot_geometry_msgs::PolygonStamped*>(handle);
+}
+
+extern "C" void navigation_free_order(OrderHandle handle) {
+    delete static_cast<robot_protocol_msgs::Order*>(handle);
+}
+
+extern "C" void navigation_free_named_occupancy_grids(NamedOccupancyGrid* maps, size_t count) {
+    if (!maps) {
+        return;
+    }
+    for (size_t i = 0; i < count; ++i) {
+        free(maps[i].name);
+        delete static_cast<robot_nav_msgs::OccupancyGrid*>(maps[i].map);
+    }
+    free(maps);
+}
+
+extern "C" void navigation_free_named_laser_scans(NamedLaserScan* scans, size_t count) {
+    if (!scans) {
+        return;
+    }
+    for (size_t i = 0; i < count; ++i) {
+        free(scans[i].name);
+        delete static_cast<robot_sensor_msgs::LaserScan*>(scans[i].scan);
+    }
+    free(scans);
+}
+
+extern "C" void navigation_free_named_point_clouds(NamedPointCloud* clouds, size_t count) {
+    if (!clouds) {
+        return;
+    }
+    for (size_t i = 0; i < count; ++i) {
+        free(clouds[i].name);
+        delete static_cast<robot_sensor_msgs::PointCloud*>(clouds[i].cloud);
+    }
+    free(clouds);
+}
+
+extern "C" void navigation_free_named_point_cloud2s(NamedPointCloud2* clouds, size_t count) {
+    if (!clouds) {
+        return;
+    }
+    for (size_t i = 0; i < count; ++i) {
+        free(clouds[i].name);
+        delete static_cast<robot_sensor_msgs::PointCloud2*>(clouds[i].cloud);
+    }
+    free(clouds);
+}
+
+extern "C" void navigation_free_planner_data(PlannerDataOutput* data) {
+    clear_planner_data(data);
+}
+
+// ============================================================================
+// Navigation Data Management
+// ============================================================================
+
+extern "C" bool navigation_add_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle map) {
+    if (!handle || !map_name || !map) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* map_ptr = static_cast<const robot_nav_msgs::OccupancyGrid*>(map);
+        nav->addStaticMap(std::string(map_name), *map_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle laser_scan) {
+    if (!handle || !laser_scan_name || !laser_scan) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* scan_ptr = static_cast<const robot_sensor_msgs::LaserScan*>(laser_scan);
+        nav->addLaserScan(std::string(laser_scan_name), *scan_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle point_cloud) {
+    if (!handle || !point_cloud_name || !point_cloud) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* cloud_ptr = static_cast<const robot_sensor_msgs::PointCloud*>(point_cloud);
+        nav->addPointCloud(std::string(point_cloud_name), *cloud_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle point_cloud2) {
+    if (!handle || !point_cloud2_name || !point_cloud2) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* cloud_ptr = static_cast<const robot_sensor_msgs::PointCloud2*>(point_cloud2);
+        nav->addPointCloud2(std::string(point_cloud2_name), *cloud_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_add_odometry(NavigationHandle handle, const char* odometry_name, OdometryHandle odometry) {
+    if (!handle || !odometry_name || !odometry) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* odom_ptr = static_cast<const robot_nav_msgs::Odometry*>(odometry);
+        nav->addOdometry(std::string(odometry_name), *odom_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle* out_map) {
+    if (!handle || !map_name || !out_map) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_nav_msgs::OccupancyGrid map = nav->getStaticMap(std::string(map_name));
+        auto* map_ptr = clone_message(map);
+        if (!map_ptr) {
+            return false;
+        }
+        *out_map = static_cast<OccupancyGridHandle>(map_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle* out_scan) {
+    if (!handle || !laser_scan_name || !out_scan) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_sensor_msgs::LaserScan scan = nav->getLaserScan(std::string(laser_scan_name));
+        auto* scan_ptr = clone_message(scan);
+        if (!scan_ptr) {
+            return false;
+        }
+        *out_scan = static_cast<LaserScanHandle>(scan_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle* out_cloud) {
+    if (!handle || !point_cloud_name || !out_cloud) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_sensor_msgs::PointCloud cloud = nav->getPointCloud(std::string(point_cloud_name));
+        auto* cloud_ptr = clone_message(cloud);
+        if (!cloud_ptr) {
+            return false;
+        }
+        *out_cloud = static_cast<PointCloudHandle>(cloud_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle* out_cloud) {
+    if (!handle || !point_cloud2_name || !out_cloud) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_sensor_msgs::PointCloud2 cloud = nav->getPointCloud2(std::string(point_cloud2_name));
+        auto* cloud_ptr = clone_message(cloud);
+        if (!cloud_ptr) {
+            return false;
+        }
+        *out_cloud = static_cast<PointCloud2Handle>(cloud_ptr);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid** out_maps, size_t* out_count) {
+    if (!handle || !out_maps || !out_count) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto maps = nav->getAllStaticMaps();
+        if (maps.empty()) {
+            *out_maps = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        NamedOccupancyGrid* result = static_cast<NamedOccupancyGrid*>(malloc(sizeof(NamedOccupancyGrid) * maps.size()));
+        if (!result) {
+            return false;
+        }
+
+        size_t index = 0;
+        for (const auto& entry : maps) {
+            result[index].name = strdup(entry.first.c_str());
+            result[index].map = static_cast<OccupancyGridHandle>(clone_message(entry.second));
+            if (!result[index].name || !result[index].map) {
+                navigation_free_named_occupancy_grids(result, index + 1);
+                return false;
+            }
+            ++index;
+        }
+
+        *out_maps = result;
+        *out_count = maps.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan** out_scans, size_t* out_count) {
+    if (!handle || !out_scans || !out_count) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto scans = nav->getAllLaserScans();
+        if (scans.empty()) {
+            *out_scans = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        NamedLaserScan* result = static_cast<NamedLaserScan*>(malloc(sizeof(NamedLaserScan) * scans.size()));
+        if (!result) {
+            return false;
+        }
+
+        size_t index = 0;
+        for (const auto& entry : scans) {
+            result[index].name = strdup(entry.first.c_str());
+            result[index].scan = static_cast<LaserScanHandle>(clone_message(entry.second));
+            if (!result[index].name || !result[index].scan) {
+                navigation_free_named_laser_scans(result, index + 1);
+                return false;
+            }
+            ++index;
+        }
+
+        *out_scans = result;
+        *out_count = scans.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud** out_clouds, size_t* out_count) {
+    if (!handle || !out_clouds || !out_count) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto clouds = nav->getAllPointClouds();
+        if (clouds.empty()) {
+            *out_clouds = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        NamedPointCloud* result = static_cast<NamedPointCloud*>(malloc(sizeof(NamedPointCloud) * clouds.size()));
+        if (!result) {
+            return false;
+        }
+
+        size_t index = 0;
+        for (const auto& entry : clouds) {
+            result[index].name = strdup(entry.first.c_str());
+            result[index].cloud = static_cast<PointCloudHandle>(clone_message(entry.second));
+            if (!result[index].name || !result[index].cloud) {
+                navigation_free_named_point_clouds(result, index + 1);
+                return false;
+            }
+            ++index;
+        }
+
+        *out_clouds = result;
+        *out_count = clouds.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2** out_clouds, size_t* out_count) {
+    if (!handle || !out_clouds || !out_count) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto clouds = nav->getAllPointCloud2s();
+        if (clouds.empty()) {
+            *out_clouds = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        NamedPointCloud2* result = static_cast<NamedPointCloud2*>(malloc(sizeof(NamedPointCloud2) * clouds.size()));
+        if (!result) {
+            return false;
+        }
+
+        size_t index = 0;
+        for (const auto& entry : clouds) {
+            result[index].name = strdup(entry.first.c_str());
+            result[index].cloud = static_cast<PointCloud2Handle>(clone_message(entry.second));
+            if (!result[index].name || !result[index].cloud) {
+                navigation_free_named_point_cloud2s(result, index + 1);
+                return false;
+            }
+            ++index;
+        }
+
+        *out_clouds = result;
+        *out_count = clouds.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char* map_name) {
+    if (!handle || !map_name) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeStaticMap(std::string(map_name));
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char* laser_scan_name) {
+    if (!handle || !laser_scan_name) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeLaserScan(std::string(laser_scan_name));
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char* point_cloud_name) {
+    if (!handle || !point_cloud_name) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removePointCloud(std::string(point_cloud_name));
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char* point_cloud2_name) {
+    if (!handle || !point_cloud2_name) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removePointCloud2(std::string(point_cloud2_name));
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) {
+    if (!handle) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeAllStaticMaps();
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) {
+    if (!handle) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeAllLaserScans();
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) {
+    if (!handle) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeAllPointClouds();
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) {
+    if (!handle) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeAllPointCloud2s();
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_remove_all_data(NavigationHandle handle) {
+    if (!handle) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        return nav->removeAllData();
+    } catch (...) {
+        return false;
+    }
+}
+
+// ============================================================================
+// Planner Data
+// ============================================================================
+
+extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput* out_data) {
+    if (!handle || !out_data) {
+        return false;
+    }
+    clear_planner_data(out_data);
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::PlannerDataOutput data = nav->getGlobalData();
+        out_data->plan = static_cast<Path2DHandle>(clone_message(data.plan));
+        out_data->costmap = static_cast<OccupancyGridHandle>(clone_message(data.costmap));
+        out_data->costmap_update = static_cast<OccupancyGridUpdateHandle>(clone_message(data.costmap_update));
+        out_data->footprint = static_cast<PolygonStampedHandle>(clone_message(data.footprint));
+        out_data->is_costmap_updated = data.is_costmap_updated;
+
+        if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) {
+            clear_planner_data(out_data);
+            return false;
+        }
+        return true;
+    } catch (...) {
+        clear_planner_data(out_data);
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput* out_data) {
+    if (!handle || !out_data) {
+        return false;
+    }
+    clear_planner_data(out_data);
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::PlannerDataOutput data = nav->getLocalData();
+        out_data->plan = static_cast<Path2DHandle>(clone_message(data.plan));
+        out_data->costmap = static_cast<OccupancyGridHandle>(clone_message(data.costmap));
+        out_data->costmap_update = static_cast<OccupancyGridUpdateHandle>(clone_message(data.costmap_update));
+        out_data->footprint = static_cast<PolygonStampedHandle>(clone_message(data.footprint));
+        out_data->is_costmap_updated = data.is_costmap_updated;
+
+        if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) {
+            clear_planner_data(out_data);
+            return false;
+        }
+        return true;
+    } catch (...) {
+        clear_planner_data(out_data);
+        return false;
+    }
+}
+
+// ============================================================================
+// Navigation Commands with Order
+// ============================================================================
+
+extern "C" bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order,
+                                         const PoseStamped* goal,
+                                         double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !order || !goal) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* order_ptr = static_cast<const robot_protocol_msgs::Order*>(order);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->moveTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
+                                         const PoseStamped* goal,
+                                         double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !order || !goal) {
+        return false;
+    }
+    try {
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* order_ptr = static_cast<const robot_protocol_msgs::Order*>(order);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->dockTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+