Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Sensor/Imu.cs

@@ -0,0 +1,128 @@
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.Shared.Sensor;
+
+/// <summary>
+/// Imu message (sensor_msgs/Imu)
+/// Contains data from an IMU (Inertial Measurement Unit)
+/// 
+/// Accelerations should be in m/s^2 (not g's), and rotational velocity should be in rad/sec
+/// 
+/// If the covariance of the measurement is known, it should be filled in (if all you know is the 
+/// variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
+/// A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
+/// data a covariance will have to be assumed or gotten from some other source
+/// 
+/// If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
+/// estimate), please set element 0 of the associated covariance matrix to -1
+/// If you are interpreting this message, please check for a value of -1 in the first element of each
+/// covariance matrix, and ignore the associated estimate.
+/// </summary>
+public struct Imu
+{
+    /// <summary>
+    /// Header with timestamp and frame ID
+    /// </summary>
+    public Header Header { get; set; }
+
+    /// <summary>
+    /// Orientation quaternion (geometry_msgs/Quaternion)
+    /// </summary>
+    public Quaternion Orientation { get; set; }
+
+    /// <summary>
+    /// Orientation covariance matrix (row-major about x, y, z axes)
+    /// Size: 9 (3x3 matrix)
+    /// </summary>
+    public double[] OrientationCovariance { get; set; }
+
+    /// <summary>
+    /// Size of orientation covariance matrix (3x3 = 9 elements)
+    /// </summary>
+    public const int OrientationCovarianceSize = 9;
+
+    /// <summary>
+    /// Angular velocity (geometry_msgs/Vector3)
+    /// </summary>
+    public Vector3 AngularVelocity { get; set; }
+
+    /// <summary>
+    /// Angular velocity covariance matrix (row-major about x, y, z axes)
+    /// Size: 9 (3x3 matrix)
+    /// </summary>
+    public double[] AngularVelocityCovariance { get; set; }
+
+    /// <summary>
+    /// Size of angular velocity covariance matrix (3x3 = 9 elements)
+    /// </summary>
+    public const int AngularVelocityCovarianceSize = 9;
+
+    /// <summary>
+    /// Linear acceleration (geometry_msgs/Vector3)
+    /// </summary>
+    public Vector3 LinearAcceleration { get; set; }
+
+    /// <summary>
+    /// Linear acceleration covariance matrix (row-major about x, y, z axes)
+    /// Size: 9 (3x3 matrix)
+    /// </summary>
+    public double[] LinearAccelerationCovariance { get; set; }
+
+    /// <summary>
+    /// Size of linear acceleration covariance matrix (3x3 = 9 elements)
+    /// </summary>
+    public const int LinearAccelerationCovarianceSize = 9;
+
+    /// <summary>
+    /// Default constructor
+    /// </summary>
+    public Imu()
+    {
+        Header = new Header();
+        Orientation = new Quaternion();
+        OrientationCovariance = new double[OrientationCovarianceSize];
+        AngularVelocity = new Vector3();
+        AngularVelocityCovariance = new double[AngularVelocityCovarianceSize];
+        LinearAcceleration = new Vector3();
+        LinearAccelerationCovariance = new double[LinearAccelerationCovarianceSize];
+    }
+
+    /// <summary>
+    /// Constructor with parameters
+    /// </summary>
+    public Imu(
+        Header header,
+        Quaternion orientation,
+        double[] orientationCovariance,
+        Vector3 angularVelocity,
+        double[] angularVelocityCovariance,
+        Vector3 linearAcceleration,
+        double[] linearAccelerationCovariance)
+    {
+        Header = header;
+        Orientation = orientation;
+        
+        if (orientationCovariance == null || orientationCovariance.Length != OrientationCovarianceSize)
+        {
+            throw new ArgumentException($"Orientation covariance array must have {OrientationCovarianceSize} elements", nameof(orientationCovariance));
+        }
+        OrientationCovariance = orientationCovariance;
+
+        AngularVelocity = angularVelocity;
+        
+        if (angularVelocityCovariance == null || angularVelocityCovariance.Length != AngularVelocityCovarianceSize)
+        {
+            throw new ArgumentException($"Angular velocity covariance array must have {AngularVelocityCovarianceSize} elements", nameof(angularVelocityCovariance));
+        }
+        AngularVelocityCovariance = angularVelocityCovariance;
+
+        LinearAcceleration = linearAcceleration;
+        
+        if (linearAccelerationCovariance == null || linearAccelerationCovariance.Length != LinearAccelerationCovarianceSize)
+        {
+            throw new ArgumentException($"Linear acceleration covariance array must have {LinearAccelerationCovarianceSize} elements", nameof(linearAccelerationCovariance));
+        }
+        LinearAccelerationCovariance = linearAccelerationCovariance;
+    }
+}
+