update c_api
This commit is contained in:
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user