Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Geometry/PoseWithCovariance.cs

@@ -0,0 +1,49 @@
+namespace RobotNet10.Shared.Geometry;
+
+/// <summary>
+/// PoseWithCovariance message (geometry_msgs/PoseWithCovariance)
+/// A Pose with associated covariance matrix
+/// </summary>
+public struct PoseWithCovariance
+{
+    /// <summary>
+    /// Pose
+    /// </summary>
+    public Pose Pose { get; set; }
+
+    /// <summary>
+    /// Covariance matrix (row-major order)
+    /// The orientation parameters use a fixed-axis representation.
+    /// In order, the parameters are:
+    /// (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+    /// </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 PoseWithCovariance()
+    {
+        Pose = new Pose();
+        Covariance = new double[CovarianceSize];
+    }
+
+    /// <summary>
+    /// Constructor with parameters
+    /// </summary>
+    public PoseWithCovariance(Pose pose, double[] covariance)
+    {
+        Pose = pose;
+        if (covariance == null || covariance.Length != CovarianceSize)
+        {
+            throw new ArgumentException($"Covariance array must have {CovarianceSize} elements", nameof(covariance));
+        }
+        Covariance = covariance;
+    }
+}
+