218 lines
8.0 KiB
C#
218 lines
8.0 KiB
C#
using System.Runtime.InteropServices;
|
|
|
|
namespace RobotNet10.RobotApp.Xloc;
|
|
|
|
/// <summary>
|
|
/// P/Invoke declarations for xloc C API
|
|
/// </summary>
|
|
public static class XlocNativeInterface
|
|
{
|
|
// Library path - adjust/ if needed
|
|
private const string LibraryPath = "/usr/lib/libxloc.so";
|
|
|
|
#region Creation / Destruction
|
|
|
|
/// <summary>
|
|
/// Create an xloc instance
|
|
/// </summary>
|
|
/// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
|
|
/// <returns>Handle to xloc instance</returns>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr xloc_create(IntPtr tfBuffer);
|
|
|
|
/// <summary>
|
|
/// Destroy an xloc instance
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_destroy(IntPtr handle);
|
|
|
|
#endregion
|
|
|
|
#region Sensor Data Dispatch
|
|
|
|
/// <summary>
|
|
/// Dispatch laser scan data to xloc
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
|
/// <param name="scan">Pointer to laser scan struct</param>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_dispatch_laserscan(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
|
ref xloc_laserscan_t scan);
|
|
|
|
/// <summary>
|
|
/// Dispatch IMU data to xloc
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
|
/// <param name="imu">Pointer to IMU struct</param>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_dispatch_imu(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
|
ref xloc_imu_t imu);
|
|
|
|
/// <summary>
|
|
/// Dispatch odometry data to xloc
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
|
/// <param name="odom">Pointer to odometry struct</param>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_dispatch_odometry(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
|
ref xloc_odometry_t odom);
|
|
|
|
#endregion
|
|
|
|
#region Localization Control
|
|
|
|
/// <summary>
|
|
/// Activate a map
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_activate_map(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);
|
|
|
|
/// <summary>
|
|
/// Switch to a different map
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_switch_map(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName,
|
|
int useInitialPose,
|
|
ref xloc_pose_t initialPose);
|
|
|
|
/// <summary>
|
|
/// Start localization
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_start_localization(IntPtr handle);
|
|
|
|
/// <summary>
|
|
/// Stop localization
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_stop_localization(IntPtr handle);
|
|
|
|
/// <summary>
|
|
/// Start mapping
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_start_mapping(IntPtr handle);
|
|
|
|
/// <summary>
|
|
/// Stop mapping and optionally save the map
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_stop_mapping(
|
|
IntPtr handle,
|
|
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);
|
|
|
|
/// <summary>
|
|
/// Set initial pose
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_set_initial_pose(
|
|
IntPtr handle,
|
|
ref xloc_pose_t initialPose);
|
|
|
|
/// <summary>
|
|
/// Change map origin
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_change_map_origin(
|
|
IntPtr handle,
|
|
ref xloc_pose_t newMapOrigin);
|
|
|
|
/// <summary>
|
|
/// Start updating existing map
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_start_update_map(IntPtr handle);
|
|
|
|
/// <summary>
|
|
/// Stop updating map and optionally save
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_stop_update_map(
|
|
IntPtr handle,
|
|
int saveUpdatedMap);
|
|
|
|
/// <summary>
|
|
/// Reset SLAM error state
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern xloc_status_response_t xloc_reset_slam_error(IntPtr handle);
|
|
|
|
#endregion
|
|
|
|
#region Getters
|
|
|
|
/// <summary>
|
|
/// Get current pose estimate
|
|
/// </summary>
|
|
/// <returns>Pointer to pose (must be freed with xloc_free_pose)</returns>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr xloc_get_current_pose(IntPtr handle);
|
|
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr xloc_get_diagnostics(IntPtr handle);
|
|
|
|
/// <summary>
|
|
/// Get static grid map (from loaded map file)
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
/// <param name="reloadFromFile">If non-zero, reload map from file</param>
|
|
/// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr xloc_get_static_grid_map(IntPtr handle, int reloadFromFile);
|
|
|
|
/// <summary>
|
|
/// Get online grid map (from SLAM)
|
|
/// </summary>
|
|
/// <param name="handle">Handle to xloc instance</param>
|
|
/// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern IntPtr xloc_get_online_grid_map(IntPtr handle);
|
|
|
|
#endregion
|
|
|
|
#region Memory Management
|
|
|
|
/// <summary>
|
|
/// Free status response
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_free_status_response(ref xloc_status_response_t response);
|
|
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_free_diagnostics(IntPtr diagnostics);
|
|
|
|
/// <summary>
|
|
/// Free C string allocated by xloc
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_free_cstring(IntPtr str);
|
|
|
|
/// <summary>
|
|
/// Free pose allocated by xloc
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_free_pose(IntPtr pose);
|
|
|
|
/// <summary>
|
|
/// Free occupancy grid allocated by xloc
|
|
/// </summary>
|
|
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void xloc_free_occupancy_grid(IntPtr grid);
|
|
|
|
#endregion
|
|
}
|