ParkingRobot/MultiWheelC/StateEstimation/VehicleState.cs

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);
        }
    }
}