Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Geometry/TwistWithCovariance.cs

@@ -0,0 +1,49 @@
+namespace RobotNet10.Shared.Geometry;
+
+/// <summary>
+/// TwistWithCovariance message (geometry_msgs/TwistWithCovariance)
+/// A Twist with associated covariance matrix
+/// </summary>
+public struct TwistWithCovariance
+{
+    /// <summary>
+    /// Twist
+    /// </summary>
+    public Twist Twist { get; set; }
+
+    /// <summary>
+    /// Covariance matrix (row-major order)
+    /// The orientation parameters use a fixed-axis representation.
+    /// In order, the parameters are:
+    /// (vx, vy, vz, wx, wy, wz)
+    /// </summary>
+    public double[] Covariance { get; set; }
+
+    /// <summary>
+    /// Size of covariance matrix (6x6 = 36 elements)
+    /// </summary>
+    public const int CovarianceSize = 36;
+
+    /// <summary>
+    /// Default constructor
+    /// </summary>
+    public TwistWithCovariance()
+    {
+        Twist = new Twist();
+        Covariance = new double[CovarianceSize];
+    }
+
+    /// <summary>
+    /// Constructor with parameters
+    /// </summary>
+    public TwistWithCovariance(Twist twist, double[] covariance)
+    {
+        Twist = twist;
+        if (covariance == null || covariance.Length != CovarianceSize)
+        {
+            throw new ArgumentException($"Covariance array must have {CovarianceSize} elements", nameof(covariance));
+        }
+        Covariance = covariance;
+    }
+}
+