139 lines
4.5 KiB
C#
139 lines
4.5 KiB
C#
using System;
|
|
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)
|
|
{
|
|
EnsureFiniteNonNegative(
|
|
sampleTimestampSeconds,
|
|
nameof(sampleTimestampSeconds));
|
|
EnsureFinitePose(
|
|
poseInWorld,
|
|
nameof(poseInWorld));
|
|
EnsureFiniteTwist(
|
|
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; }
|
|
|
|
/// <summary>
|
|
/// 检查位姿是否由有限数值组成。
|
|
/// </summary>
|
|
private static void EnsureFinitePose(
|
|
Pose2D pose,
|
|
string parameterName)
|
|
{
|
|
if (!IsFinite(pose.XMeters) ||
|
|
!IsFinite(pose.YMeters) ||
|
|
!IsFinite(pose.YawRadians))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"车辆位姿必须由有限数值组成。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查速度是否由有限数值组成。
|
|
/// </summary>
|
|
private static void EnsureFiniteTwist(
|
|
Twist2D twist,
|
|
string parameterName)
|
|
{
|
|
if (!IsFinite(twist.VxMetersPerSecond) ||
|
|
!IsFinite(twist.VyMetersPerSecond) ||
|
|
!IsFinite(twist.OmegaRadiansPerSecond))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"车辆速度必须由有限数值组成。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查数值是否为非负有限值。
|
|
/// </summary>
|
|
private static void EnsureFiniteNonNegative(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (!IsFinite(value) || value < 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"采样时刻必须是非负有限值。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断数值是否可用于车辆状态计算。
|
|
/// </summary>
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) &&
|
|
!double.IsInfinity(value);
|
|
}
|
|
}
|
|
}
|