295 lines
16 KiB
C#
295 lines
16 KiB
C#
using System;
|
|
using System.Runtime.InteropServices;
|
|
using NavigationExample;
|
|
|
|
namespace NavigationExample
|
|
{
|
|
/// <summary>
|
|
/// C# P/Invoke wrapper for Navigation C API
|
|
/// </summary>
|
|
public class NavigationAPI
|
|
{
|
|
private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
|
|
// For Windows: "nav_c_api.dll"
|
|
// For macOS: "libnav_c_api.dylib"
|
|
|
|
// ============================================================================
|
|
// Enums
|
|
// ============================================================================
|
|
public enum NavigationState
|
|
{
|
|
Pending = 0,
|
|
Active = 1,
|
|
Preempted = 2,
|
|
Succeeded = 3,
|
|
Aborted = 4,
|
|
Rejected = 5,
|
|
Preempting = 6,
|
|
Recalling = 7,
|
|
Recalled = 8,
|
|
Lost = 9,
|
|
Planning = 10,
|
|
Controlling = 11,
|
|
Clearing = 12,
|
|
Paused = 13
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct NavFeedback
|
|
{
|
|
public NavigationState navigation_state;
|
|
public IntPtr feed_back_str; // char*; free with nav_c_api_free_string
|
|
public Pose2D current_pose;
|
|
[MarshalAs(UnmanagedType.I1)]
|
|
public bool goal_checked;
|
|
[MarshalAs(UnmanagedType.I1)]
|
|
public bool is_ready;
|
|
}
|
|
|
|
|
|
/// <summary>Planner data output (plan, costmap, footprint).</summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct PlannerDataOutput
|
|
{
|
|
public Path2D plan;
|
|
public OccupancyGrid costmap;
|
|
public OccupancyGridUpdate costmap_update;
|
|
[MarshalAs(UnmanagedType.I1)]
|
|
public bool is_costmap_updated;
|
|
public PolygonStamped footprint;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct NavigationHandle
|
|
{
|
|
public IntPtr ptr;
|
|
}
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern Header header_create(string frame_id);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern Header header_set_data(
|
|
uint seq,
|
|
uint sec,
|
|
uint nsec,
|
|
string frame_id);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern Time time_create();
|
|
|
|
/// <summary>Free a string allocated by the API (strdup).</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void nav_c_api_free_string(IntPtr str);
|
|
|
|
/// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern IntPtr navigation_state_to_string(NavigationState state);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
|
|
|
|
|
|
/// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
|
|
public static string MarshalString(IntPtr p)
|
|
{
|
|
if (p == IntPtr.Zero) return string.Empty;
|
|
return Marshal.PtrToStringAnsi(p) ?? string.Empty;
|
|
}
|
|
|
|
/// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
|
|
public static void navigation_free_feedback(ref NavFeedback feedback)
|
|
{
|
|
if (feedback.feed_back_str != IntPtr.Zero)
|
|
{
|
|
nav_c_api_free_string(feedback.feed_back_str);
|
|
feedback.feed_back_str = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Navigation Handle Management
|
|
// ============================================================================
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern NavigationHandle navigation_create();
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void navigation_destroy(NavigationHandle handle);
|
|
|
|
/// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
|
|
|
|
/// <summary>Send a goal for the robot to navigate to (global frame).</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);
|
|
|
|
/// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
|
|
|
|
/// <summary>Rotate in place to align with target orientation (radians).</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_pause(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_resume(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_cancel(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
|
|
|
|
// ============================================================================
|
|
// Navigation Data Management
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_laser_scan(NavigationHandle 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(NavigationHandle 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(NavigationHandle handle, string point_cloud2_name);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_remove_all_data(NavigationHandle handle);
|
|
|
|
/// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
|
|
}
|
|
} |