Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocInterop.cs

@@ -0,0 +1,162 @@
+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
+}