65 lines
2.3 KiB
C#
65 lines
2.3 KiB
C#
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Fleet
|
|
{
|
|
// 一次经过校验的虚拟车队原点状态快照。
|
|
public readonly struct FleetState
|
|
{
|
|
public FleetState(
|
|
double sampleTimestampSeconds,
|
|
Pose2D fleetPoseInWorld,
|
|
Twist2D twistAtFleetOriginInWorld,
|
|
bool hasValidVelocityEstimate)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
sampleTimestampSeconds,
|
|
nameof(sampleTimestampSeconds));
|
|
NumericGuard.EnsureFinite(
|
|
fleetPoseInWorld,
|
|
nameof(fleetPoseInWorld));
|
|
NumericGuard.EnsureFinite(
|
|
twistAtFleetOriginInWorld,
|
|
nameof(twistAtFleetOriginInWorld));
|
|
|
|
SampleTimestampSeconds =
|
|
sampleTimestampSeconds;
|
|
FleetPoseInWorld = new Pose2D(
|
|
fleetPoseInWorld.XMeters,
|
|
fleetPoseInWorld.YMeters,
|
|
AngleMath.NormalizeRadians(
|
|
fleetPoseInWorld.YawRadians));
|
|
HasValidVelocityEstimate =
|
|
hasValidVelocityEstimate;
|
|
|
|
// 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
|
|
TwistAtFleetOriginInWorld =
|
|
hasValidVelocityEstimate
|
|
? twistAtFleetOriginInWorld
|
|
: Twist2D.Zero;
|
|
|
|
var worldPoseInFleet =
|
|
FrameTransform2D.Inverse(
|
|
FleetPoseInWorld);
|
|
TwistAtFleetOriginInFleet =
|
|
FrameTransform2D.TransformTwistAtSamePoint(
|
|
worldPoseInFleet,
|
|
TwistAtFleetOriginInWorld);
|
|
}
|
|
|
|
// 状态源单调时钟中的采样时刻，单位为s。
|
|
public double SampleTimestampSeconds { get; }
|
|
|
|
// 车队坐标系原点在世界坐标系中的实际位姿。
|
|
public Pose2D FleetPoseInWorld { get; }
|
|
|
|
// 车队原点处的实际刚体速度，在世界坐标系中表达。
|
|
public Twist2D TwistAtFleetOriginInWorld { get; }
|
|
|
|
// 同一刚体速度在车队坐标系中表达，供车队控制器使用。
|
|
public Twist2D TwistAtFleetOriginInFleet { get; }
|
|
|
|
// 速度是否已经初始化并可用于闭环控制。
|
|
public bool HasValidVelocityEstimate { get; }
|
|
}
|
|
}
|