50 lines
1.3 KiB
C#
50 lines
1.3 KiB
C#
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;
|
|
}
|
|
}
|
|
|