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/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;
+    }
+}
+