81 lines
2.7 KiB
C#
81 lines
2.7 KiB
C#
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.StateEstimation
|
|
{
|
|
/// <summary>
|
|
/// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
|
|
/// </summary>
|
|
public readonly struct VehicleState
|
|
{
|
|
/// <summary>
|
|
/// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
|
|
/// </summary>
|
|
public VehicleState(
|
|
double sampleTimestampSeconds,
|
|
Pose2D poseInWorld,
|
|
Twist2D twistInWorld,
|
|
bool hasValidVelocityEstimate)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
sampleTimestampSeconds,
|
|
nameof(sampleTimestampSeconds));
|
|
NumericGuard.EnsureFinite(
|
|
poseInWorld,
|
|
nameof(poseInWorld));
|
|
NumericGuard.EnsureFinite(
|
|
twistInWorld,
|
|
nameof(twistInWorld));
|
|
|
|
SampleTimestampSeconds =
|
|
sampleTimestampSeconds;
|
|
PoseInWorld = new Pose2D(
|
|
poseInWorld.XMeters,
|
|
poseInWorld.YMeters,
|
|
AngleMath.NormalizeRadians(
|
|
poseInWorld.YawRadians));
|
|
HasValidVelocityEstimate =
|
|
hasValidVelocityEstimate;
|
|
|
|
// 第一帧或定位重置后的速度不可用于闭环控制，
|
|
// 此时显式置零，避免调用方误用残留速度。
|
|
TwistInWorld = hasValidVelocityEstimate
|
|
? twistInWorld
|
|
: Twist2D.Zero;
|
|
|
|
var worldPoseInBody =
|
|
FrameTransform2D.Inverse(
|
|
PoseInWorld);
|
|
TwistInBody =
|
|
FrameTransform2D.TransformTwistAtSamePoint(
|
|
worldPoseInBody,
|
|
TwistInWorld);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取状态源单调时钟中的采样时刻，单位为s。
|
|
/// </summary>
|
|
public double SampleTimestampSeconds { get; }
|
|
|
|
/// <summary>
|
|
/// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
|
|
/// </summary>
|
|
public Pose2D PoseInWorld { get; }
|
|
|
|
/// <summary>
|
|
/// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
|
|
/// </summary>
|
|
public Twist2D TwistInWorld { get; }
|
|
|
|
/// <summary>
|
|
/// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
|
|
/// </summary>
|
|
public Twist2D TwistInBody { get; }
|
|
|
|
/// <summary>
|
|
/// 获取当前速度估计是否已经初始化并可用于闭环控制。
|
|
/// </summary>
|
|
public bool HasValidVelocityEstimate { get; }
|
|
|
|
}
|
|
}
|