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,7 +2,8 @@
 #define NAVIGATION_C_API_H
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
 
 #include <stdbool.h>
@@ -12,6 +13,15 @@ extern "C" {
   // Forward declarations
   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
@@ -20,7 +30,8 @@ typedef void* TFListenerHandle;
   /**
    * @brief Navigation states, including planning and controller status
    */
-typedef enum {
+  typedef enum
+  {
     NAV_STATE_PENDING = 0,
     NAV_STATE_ACTIVE = 1,
     NAV_STATE_PREEMPTED = 2,
@@ -44,7 +55,8 @@ typedef enum {
   /**
    * @brief Point structure (x, y, z)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
@@ -53,7 +65,8 @@ typedef struct {
   /**
    * @brief Pose2D structure (x, y, theta)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double theta;
@@ -62,7 +75,8 @@ typedef struct {
   /**
    * @brief Twist2D structure (x, y, theta velocities)
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double theta;
@@ -71,7 +85,8 @@ typedef struct {
   /**
    * @brief Quaternion structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
@@ -81,7 +96,8 @@ typedef struct {
   /**
    * @brief Position structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
@@ -90,7 +106,8 @@ typedef struct {
   /**
    * @brief Pose structure
    */
-typedef struct {
+  typedef struct
+  {
     Position position;
     Quaternion orientation;
   } Pose;
@@ -98,7 +115,8 @@ typedef struct {
   /**
    * @brief Header structure
    */
-typedef struct {
+  typedef struct
+  {
     uint32_t seq;
     int64_t sec;
     uint32_t nsec;
@@ -108,7 +126,8 @@ typedef struct {
   /**
    * @brief PoseStamped structure
    */
-typedef struct {
+  typedef struct
+  {
     Header header;
     Pose pose;
   } PoseStamped;
@@ -116,7 +135,8 @@ typedef struct {
   /**
    * @brief Twist2DStamped structure
    */
-typedef struct {
+  typedef struct
+  {
     Header header;
     Twist2D velocity;
   } Twist2DStamped;
@@ -124,7 +144,8 @@ typedef struct {
   /**
    * @brief Vector3 structure
    */
-typedef struct {
+  typedef struct
+  {
     double x;
     double y;
     double z;
@@ -133,7 +154,8 @@ typedef struct {
   /**
    * @brief Navigation feedback structure
    */
-typedef struct {
+  typedef struct
+  {
     NavigationState navigation_state;
     char *feed_back_str;
     Pose2D current_pose;
@@ -141,6 +163,58 @@ typedef struct {
     bool is_ready;
   } NavFeedback;
 
+  /**
+   * @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
   // ============================================================================
@@ -151,6 +225,97 @@ typedef struct {
    */
   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
   // ============================================================================
@@ -293,6 +458,19 @@ void navigation_free_points(Point* points);
   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
@@ -306,6 +484,19 @@ bool navigation_dock_to(NavigationHandle handle, const char* marker,
                           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
@@ -406,9 +597,208 @@ bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_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;
+    }
+}
+