ParkingRobot/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

using System;
using CommonUsage.Chassis;
using MyParking.Shared;
using System.Diagnostics;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
    /// </summary>
    public sealed class WheelFeedbackVehicleStateProvider
        : IVehicleStateProvider
    {
        private readonly Stopwatch _wheelSpeedClock = Stopwatch.StartNew();
        public const double DefaultVelocityFilterTimeConstantSeconds =
            0.10;

        private readonly object _syncRoot = new object();
        private readonly IVehicleStateProvider _poseProvider;
        private readonly MultiWheelChassis _chassis;
        private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;

        private bool _hasPreviousTimestamp;
        private double _previousTimestampSeconds;
        private bool _hasVelocityDiagnostics;
        private double _latestDetourBodyVxMetersPerSecond;
        private bool _latestDetourVelocityValid;
        private double _latestRawWheelBodyVxMetersPerSecond;
        private double _latestFilteredWheelBodyVxMetersPerSecond;
        private bool _latestWheelVelocityValid;

        /// <summary>
        /// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
        /// </summary>
        public WheelFeedbackVehicleStateProvider(
            IVehicleStateProvider poseProvider,
            MultiWheelChassis chassis)
            : this(
                poseProvider,
                chassis,
                DefaultVelocityFilterTimeConstantSeconds)
        {
        }

        /// <summary>
        /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
        /// </summary>
        public WheelFeedbackVehicleStateProvider(
            IVehicleStateProvider poseProvider,
            MultiWheelChassis chassis,
            double velocityFilterTimeConstantSeconds)
        {
            _poseProvider = poseProvider ??
                throw new ArgumentNullException(
                    nameof(poseProvider));
            _chassis = chassis ??
                throw new ArgumentNullException(
                    nameof(chassis));
            _longitudinalSpeedFilter =
                new FirstOrderLowPassFilter(
                    velocityFilterTimeConstantSeconds);
        }

        /// <summary>
        /// 获取最近一次读取失败的原因，正常时为空字符串。
        /// </summary>
        public string LastFailureReason { get; private set; } =
            string.Empty;

        /// <summary>
        /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
        /// </summary>
        public bool TryGetState(out VehicleState state)
        {
            lock (_syncRoot)
            {
                if (!_poseProvider.TryGetState(
                        out var poseState))
                {
                    state = default;
                    LastFailureReason =
                        "基础位姿状态源暂时不可用。";
                    return false;
                }

                try
                {
                    var actualCarSpeed =
                        _chassis.GetCarSpeed(true);
                    var rawLongitudinalSpeedMetersPerSecond =
                        (double)actualCarSpeed.Vx;

                    NumericGuard.EnsureFinite(
                        rawLongitudinalSpeedMetersPerSecond,
                        "电机反馈车体纵向速度");

                    var wheelSpeedTimestampSeconds =
                        _wheelSpeedClock.Elapsed.TotalSeconds;

                    var filteredLongitudinalSpeedMetersPerSecond =
                        UpdateLongitudinalSpeedFilter(
                            rawLongitudinalSpeedMetersPerSecond,
                            wheelSpeedTimestampSeconds,
                            out var hasValidWheelSpeedEstimate);

                    _latestDetourBodyVxMetersPerSecond =
                        poseState.TwistInBody.VxMetersPerSecond;
                    _latestDetourVelocityValid =
                        poseState.HasValidVelocityEstimate;
                    _latestRawWheelBodyVxMetersPerSecond =
                        rawLongitudinalSpeedMetersPerSecond;
                    _latestFilteredWheelBodyVxMetersPerSecond =
                        filteredLongitudinalSpeedMetersPerSecond;
                    _latestWheelVelocityValid =
                        hasValidWheelSpeedEstimate;
                    _hasVelocityDiagnostics = true;

                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
                    var twistInBody = new Twist2D(
                        filteredLongitudinalSpeedMetersPerSecond,
                        poseState.TwistInBody
                            .VyMetersPerSecond,
                        poseState.TwistInBody
                            .OmegaRadiansPerSecond);

                    var twistInWorld =
                        FrameTransform2D
                            .TransformTwistAtSamePoint(
                                poseState.PoseInWorld,
                                twistInBody);

                    state = new VehicleState(
                        poseState.SampleTimestampSeconds,
                        poseState.PoseInWorld,
                        twistInWorld,
                        hasValidWheelSpeedEstimate);

                    LastFailureReason = string.Empty;
                    return true;
                }
                catch (Exception exception)
                {
                    state = default;
                    LastFailureReason =
                        "舵轮电机反馈车体速度解算失败：" +
                        exception.Message;
                    return false;
                }
            }
        }

        /// <summary>
        /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
        /// </summary>
        public bool TryGetLatestVelocityDiagnostics(
            out double detourBodyVxMetersPerSecond,
            out bool detourVelocityValid,
            out double rawWheelBodyVxMetersPerSecond,
            out double filteredWheelBodyVxMetersPerSecond,
            out bool wheelVelocityValid)
        {
            lock (_syncRoot)
            {
                detourBodyVxMetersPerSecond =
                    _latestDetourBodyVxMetersPerSecond;
                detourVelocityValid =
                    _latestDetourVelocityValid;
                rawWheelBodyVxMetersPerSecond =
                    _latestRawWheelBodyVxMetersPerSecond;
                filteredWheelBodyVxMetersPerSecond =
                    _latestFilteredWheelBodyVxMetersPerSecond;
                wheelVelocityValid =
                    _latestWheelVelocityValid;
                return _hasVelocityDiagnostics;
            }
        }

        /// <summary>
        /// 清除电机反馈速度的时间基准和低通滤波历史。
        /// </summary>
        public void Reset()
        {
            lock (_syncRoot)
            {
                _longitudinalSpeedFilter.Reset();
                _wheelSpeedClock.Restart();
                _hasPreviousTimestamp = false;
                _previousTimestampSeconds = 0.0;
                _hasVelocityDiagnostics = false;
                _latestDetourBodyVxMetersPerSecond = 0.0;
                _latestDetourVelocityValid = false;
                _latestRawWheelBodyVxMetersPerSecond = 0.0;
                _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
                _latestWheelVelocityValid = false;
                LastFailureReason = string.Empty;
            }
        }

        /// <summary>
        /// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
        /// </summary>
        private double UpdateLongitudinalSpeedFilter(
            double rawLongitudinalSpeedMetersPerSecond,
            double timestampSeconds,
            out bool hasValidWheelSpeedEstimate)
        {
            NumericGuard.EnsureFiniteNonNegative(
                timestampSeconds,
                nameof(timestampSeconds));

            if (!_hasPreviousTimestamp)
            {
                _longitudinalSpeedFilter.Reset(
                    rawLongitudinalSpeedMetersPerSecond);
                _previousTimestampSeconds = timestampSeconds;
                _hasPreviousTimestamp = true;
                hasValidWheelSpeedEstimate = false;
                return rawLongitudinalSpeedMetersPerSecond;
            }

            var deltaTimeSeconds =
                timestampSeconds -
                _previousTimestampSeconds;
            _previousTimestampSeconds = timestampSeconds;

            if (deltaTimeSeconds <= 0.0)
            {
                _longitudinalSpeedFilter.Reset(
                    rawLongitudinalSpeedMetersPerSecond);
                hasValidWheelSpeedEstimate = false;
                return rawLongitudinalSpeedMetersPerSecond;
            }

            hasValidWheelSpeedEstimate = true;
            return _longitudinalSpeedFilter.Update(
                rawLongitudinalSpeedMetersPerSecond,
                deltaTimeSeconds);
        }

    }
}