129 lines
4.8 KiB
C#
129 lines
4.8 KiB
C#
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;
|
|
}
|
|
}
|
|
|