163 lines
5.3 KiB
C#
163 lines
5.3 KiB
C#
using System.Runtime.InteropServices;
|
|
using RobotNet10.RobotApp.Navigation;
|
|
|
|
namespace RobotNet10.RobotApp.Xloc;
|
|
|
|
/// <summary>
|
|
/// C-compatible structs for xloc C API interop
|
|
/// Based on /home/robotics/sonvh/xloc_Linux_0.1.0/_CPack_Packages/Linux/DEB/xloc-0.1.0-Linux/usr/include/xloc/xloc_c_api.h
|
|
/// </summary>
|
|
|
|
/// <summary>
|
|
/// Unix timestamp with seconds and nanoseconds (xloc_unix_time_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_unix_time_t
|
|
{
|
|
public uint sec;
|
|
public uint nsec;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Message header (xloc_header_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_header_t
|
|
{
|
|
public uint seq;
|
|
public xloc_unix_time_t stamp;
|
|
public IntPtr frame_id; // char* - must be allocated and freed
|
|
}
|
|
|
|
/// <summary>
|
|
/// Odometry message (xloc_odometry_t)
|
|
/// CRITICAL: Must match exact C API definition in xloc_c_api.h (lines 138-149)
|
|
/// Uses flattened arrays, NOT nested xloc_pose_t or xloc_twist_t structs
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_odometry_t
|
|
{
|
|
public xloc_header_t header;
|
|
public IntPtr child_frame_id; // char* - must be allocated and freed
|
|
|
|
// Pose with covariance (flattened, not using xloc_pose_t)
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] pose_position; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
|
public double[] pose_orientation; // [4] - x, y, z, w (quaternion)
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
|
|
public double[] pose_covariance; // [36] - 6x6 matrix
|
|
|
|
// Twist with covariance (flattened, not using xloc_twist_t)
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] twist_linear; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] twist_angular; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
|
|
public double[] twist_covariance; // [36] - 6x6 matrix
|
|
}
|
|
|
|
/// <summary>
|
|
/// IMU message (xloc_imu_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_imu_t
|
|
{
|
|
public xloc_header_t header;
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
|
public double[] orientation; // [4] - x, y, z, w (quaternion)
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
|
public double[] orientation_covariance; // [9] - 3x3 matrix
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] angular_velocity; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
|
public double[] angular_velocity_covariance; // [9] - 3x3 matrix
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] linear_acceleration; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
|
public double[] linear_acceleration_covariance; // [9] - 3x3 matrix
|
|
}
|
|
|
|
/// <summary>
|
|
/// LaserScan message (xloc_laserscan_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_laserscan_t
|
|
{
|
|
public xloc_header_t header;
|
|
public float angle_min;
|
|
public float angle_max;
|
|
public float angle_increment;
|
|
public float time_increment;
|
|
public float scan_time;
|
|
public float range_min;
|
|
public float range_max;
|
|
|
|
public IntPtr ranges; // float* - pointer to array
|
|
public nuint ranges_length; // size_t - number of elements
|
|
|
|
public IntPtr intensities; // float* - pointer to array
|
|
public nuint intensities_length; // size_t - number of elements
|
|
}
|
|
|
|
/// <summary>
|
|
/// Status response from xloc (xloc_status_response_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_status_response_t
|
|
{
|
|
public byte code;
|
|
public IntPtr message; // char* - must be freed with xloc_free_cstring
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pose (xloc_pose_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_pose_t
|
|
{
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
|
public double[] position; // [3] - x, y, z
|
|
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
|
public double[] orientation; // [4] - x, y, z, w (quaternion)
|
|
}
|
|
|
|
/// <summary>
|
|
/// Diagnostics information from xloc (xloc_diagnostics_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_diagnostics_t
|
|
{
|
|
public xloc_header_t header;
|
|
public byte xloc_state; // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR
|
|
public IntPtr current_active_map; // char* - must be freed
|
|
public double reliability; // 0.0 to 1.0
|
|
public double matching_score; // SLAM matching quality
|
|
}
|
|
|
|
/// <summary>
|
|
/// Occupancy grid map (xloc_occupancy_grid_t)
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct xloc_occupancy_grid_t
|
|
{
|
|
public xloc_header_t header;
|
|
public float resolution; // meters per cell
|
|
public uint width; // cells
|
|
public uint height; // cells
|
|
public xloc_pose_t origin; // pose of cell (0,0) in map frame
|
|
public IntPtr data; // int8_t* - pointer to occupancy data (width*height bytes)
|
|
public nuint data_length; // size_t - number of bytes
|
|
}
|