HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update c_api

Browse Source
This commit is contained in:
HiepLM 2026-02-04 14:33:51 +07:00
parent fd022bd9d7
commit cab5655769
5 changed files with 1785 additions and 305 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

240
examples/CSharpExample.cs
View File

@ -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);

240
examples/NavigationExample/Program.cs
View File

@ -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);

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

644
src/APIs/c_api/include/nav_c_api.h
View File

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

610
src/APIs/c_api/src/nav_c_api.cpp
View File

@ -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;
}
}