Initial commit
This commit is contained in:
@@ -0,0 +1,217 @@
|
||||
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
|
||||
}
|
||||
Reference in New Issue
Block a user