ParkingRobot/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

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

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 保留外部状态源的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 readonly FirstOrderLowPassFilter _lateralSpeedFilter;
        private readonly FirstOrderLowPassFilter _angularSpeedFilter;

        private bool _hasPreviousTimestamp;
        private double _previousTimestampSeconds;
        private bool _hasVelocityDiagnostics;
        private double _latestDetourBodyVxMetersPerSecond;
        private bool _latestDetourVelocityValid;
        private double _latestRawWheelBodyVxMetersPerSecond;
        private double _latestFilteredWheelBodyVxMetersPerSecond;
        private double _latestRawWheelBodyVyMetersPerSecond;
        private double _latestFilteredWheelBodyVyMetersPerSecond;
        private double _latestRawWheelBodyOmegaRadiansPerSecond;
        private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
        private double _latestWheelSampleTimestampSeconds;
        private bool _latestWheelVelocityValid;
        private bool _latestWheelFeedbackReadSucceeded;

        /// <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);
            _lateralSpeedFilter =
                new FirstOrderLowPassFilter(
                    velocityFilterTimeConstantSeconds);
            _angularSpeedFilter =
                new FirstOrderLowPassFilter(
                    velocityFilterTimeConstantSeconds);
        }

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

        /// <summary>
        /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
        /// </summary>
        public string LastHeadingFailureReason { get; private set; } =
            string.Empty;

        /// <summary>
        /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
        /// </summary>
        public bool TryGetState(out VehicleState state)
        {
            lock (_syncRoot)
            {
                try
                {
                    ReadFilteredWheelTwist(
                        out var filteredWheelTwist,
                        out _,
                        out var hasValidWheelSpeedEstimate);

                    // Detour位姿跳变确认期间需要用轮速维持短时运动预测。
                    if (_poseProvider is DetourVehicleStateProvider
                            detourStateProvider)
                    {
                        detourStateProvider.UpdateWheelVelocityEstimate(
                            filteredWheelTwist.VxMetersPerSecond,
                            filteredWheelTwist.VyMetersPerSecond,
                            filteredWheelTwist.OmegaRadiansPerSecond,
                            hasValidWheelSpeedEstimate);
                    }

                    if (!_poseProvider.TryGetState(
                            out var poseState))
                    {
                        state = default;
                        LastFailureReason =
                            GetPoseProviderFailureReason();
                        return false;
                    }

                    _latestDetourBodyVxMetersPerSecond =
                        poseState.TwistInBody.VxMetersPerSecond;
                    _latestDetourVelocityValid =
                        poseState.HasValidVelocityEstimate;
                    _hasVelocityDiagnostics = true;

                    // 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
                    // 暂时保留Detour角速度作为回退值。
                    var omegaRadiansPerSecond =
                        hasValidWheelSpeedEstimate
                            ? filteredWheelTwist
                                .OmegaRadiansPerSecond
                            : poseState.TwistInBody
                                .OmegaRadiansPerSecond;
                    var twistInBody = new Twist2D(
                        filteredWheelTwist.VxMetersPerSecond,
                        filteredWheelTwist.VyMetersPerSecond,
                        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)
                {
                    _latestWheelFeedbackReadSucceeded = false;
                    state = default;
                    LastFailureReason =
                        "舵轮电机反馈车体速度解算失败：" +
                        exception.Message;
                    return false;
                }
            }
        }

        /// <summary>
        /// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。
        /// </summary>
        public bool TryGetWheelTwist(
            out Twist2D twistInBody,
            out double sampleTimestampSeconds)
        {
            lock (_syncRoot)
            {
                try
                {
                    ReadFilteredWheelTwist(
                        out var filteredWheelTwist,
                        out sampleTimestampSeconds,
                        out var hasValidWheelSpeedEstimate);

                    if (!hasValidWheelSpeedEstimate)
                    {
                        twistInBody = Twist2D.Zero;
                        LastFailureReason =
                            "轮组速度估计正在建立采样时间基准。";
                        return false;
                    }

                    twistInBody = filteredWheelTwist;
                    LastFailureReason = string.Empty;
                    return true;
                }
                catch (Exception exception)
                {
                    _latestWheelFeedbackReadSucceeded = false;
                    twistInBody = Twist2D.Zero;
                    sampleTimestampSeconds = 0.0;
                    LastFailureReason =
                        "舵轮电机反馈车体速度解算失败：" +
                        exception.Message;
                    return false;
                }
            }
        }

        /// <summary>
        /// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
        /// </summary>
        public bool TryGetHeadingRadians(
            out double headingRadians)
        {
            lock (_syncRoot)
            {
                TryGetState(out _);

                if (!_latestWheelFeedbackReadSucceeded)
                {
                    headingRadians = 0.0;
                    LastHeadingFailureReason =
                        string.IsNullOrWhiteSpace(
                            LastFailureReason)
                            ? "舵轮反馈当前不可用，无法校验航向。"
                            : LastFailureReason;
                    return false;
                }

                if (_poseProvider is DetourVehicleStateProvider
                        detourStateProvider)
                {
                    var success = detourStateProvider
                        .TryGetLatestReliableHeadingRadians(
                            out headingRadians);
                    LastHeadingFailureReason = success
                        ? string.Empty
                        : detourStateProvider
                            .LastHeadingFailureReason;
                    return success;
                }

                if (_poseProvider.TryGetState(
                        out var poseState))
                {
                    headingRadians =
                        poseState.PoseInWorld.YawRadians;
                    LastHeadingFailureReason = string.Empty;
                    return true;
                }

                headingRadians = 0.0;
                LastHeadingFailureReason =
                    GetPoseProviderFailureReason();
                return false;
            }
        }

        /// <summary>
        /// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
        /// </summary>
        public void BeginPostRotationPositionRecovery()
        {
            lock (_syncRoot)
            {
                if (_poseProvider is DetourVehicleStateProvider
                        detourStateProvider)
                {
                    detourStateProvider
                        .BeginPostRotationPositionRecovery();
                }
            }
        }

        private string GetPoseProviderFailureReason()
        {
            if (_poseProvider is DetourVehicleStateProvider
                    detourStateProvider &&
                !string.IsNullOrWhiteSpace(
                    detourStateProvider.LastFailureReason))
            {
                return detourStateProvider.LastFailureReason;
            }

            return "基础位姿状态源暂时不可用。";
        }

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

        /// <summary>
        /// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
        /// </summary>
        public bool TryGetLatestVelocityDiagnostics(
            out double detourBodyVxMetersPerSecond,
            out bool detourVelocityValid,
            out double rawWheelBodyVxMetersPerSecond,
            out double filteredWheelBodyVxMetersPerSecond,
            out double rawWheelBodyVyMetersPerSecond,
            out double filteredWheelBodyVyMetersPerSecond,
            out double rawWheelBodyOmegaRadiansPerSecond,
            out double filteredWheelBodyOmegaRadiansPerSecond,
            out double wheelSampleTimestampSeconds,
            out bool wheelVelocityValid)
        {
            lock (_syncRoot)
            {
                detourBodyVxMetersPerSecond =
                    _latestDetourBodyVxMetersPerSecond;
                detourVelocityValid =
                    _latestDetourVelocityValid;
                rawWheelBodyVxMetersPerSecond =
                    _latestRawWheelBodyVxMetersPerSecond;
                filteredWheelBodyVxMetersPerSecond =
                    _latestFilteredWheelBodyVxMetersPerSecond;
                rawWheelBodyVyMetersPerSecond =
                    _latestRawWheelBodyVyMetersPerSecond;
                filteredWheelBodyVyMetersPerSecond =
                    _latestFilteredWheelBodyVyMetersPerSecond;
                rawWheelBodyOmegaRadiansPerSecond =
                    _latestRawWheelBodyOmegaRadiansPerSecond;
                filteredWheelBodyOmegaRadiansPerSecond =
                    _latestFilteredWheelBodyOmegaRadiansPerSecond;
                wheelSampleTimestampSeconds =
                    _latestWheelSampleTimestampSeconds;
                wheelVelocityValid =
                    _latestWheelVelocityValid;
                return _hasVelocityDiagnostics;
            }
        }

        /// <summary>
        /// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
        /// </summary>
        public bool TryGetLatestDetourDiagnostics(
            out bool jumpCandidateActive,
            out int jumpCandidateConsistentFrameCount,
            out double estimatedShiftDistanceMeters,
            out double estimatedShiftHeadingRadians,
            out int automaticFrameShiftCount,
            out string stateStatusReason,
            out double detourDataAgeMilliseconds)
        {
            lock (_syncRoot)
            {
                if (_poseProvider is DetourVehicleStateProvider
                        detourStateProvider)
                {
                    var hasDiagnostics = detourStateProvider
                        .TryGetLatestDiagnostics(
                            out jumpCandidateActive,
                            out jumpCandidateConsistentFrameCount,
                            out estimatedShiftDistanceMeters,
                            out estimatedShiftHeadingRadians,
                            out automaticFrameShiftCount,
                            out stateStatusReason,
                            out detourDataAgeMilliseconds);

                    if (string.IsNullOrWhiteSpace(
                            stateStatusReason) &&
                        !string.IsNullOrWhiteSpace(
                            LastFailureReason))
                    {
                        stateStatusReason = LastFailureReason;
                    }

                    return hasDiagnostics;
                }

                jumpCandidateActive = false;
                jumpCandidateConsistentFrameCount = 0;
                estimatedShiftDistanceMeters = 0.0;
                estimatedShiftHeadingRadians = 0.0;
                automaticFrameShiftCount = 0;
                stateStatusReason = LastFailureReason;
                detourDataAgeMilliseconds = 0.0;
                return false;
            }
        }

        /// <summary>
        /// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
        /// </summary>
        public bool TryGetLatestDetourDiagnostics(
            out bool jumpCandidateActive,
            out int jumpCandidateConsistentFrameCount,
            out double estimatedShiftDistanceMeters,
            out double estimatedShiftHeadingRadians,
            out int automaticFrameShiftCount,
            out double sourceFrameIntervalSeconds,
            out double motionPredictionTimestampSeconds,
            out bool hasInnovationDiagnostics,
            out double positionInnovationMeters,
            out double allowedPositionInnovationMeters,
            out double headingInnovationRadians,
            out double allowedHeadingInnovationRadians,
            out string jumpCandidateTriggerReason,
            out string stateStatus,
            out string stateStatusReason,
            out double detourDataAgeMilliseconds)
        {
            lock (_syncRoot)
            {
                if (_poseProvider is DetourVehicleStateProvider
                        detourStateProvider)
                {
                    var hasDiagnostics = detourStateProvider
                        .TryGetLatestDiagnostics(
                            out jumpCandidateActive,
                            out jumpCandidateConsistentFrameCount,
                            out estimatedShiftDistanceMeters,
                            out estimatedShiftHeadingRadians,
                            out automaticFrameShiftCount,
                            out sourceFrameIntervalSeconds,
                            out motionPredictionTimestampSeconds,
                            out hasInnovationDiagnostics,
                            out positionInnovationMeters,
                            out allowedPositionInnovationMeters,
                            out headingInnovationRadians,
                            out allowedHeadingInnovationRadians,
                            out jumpCandidateTriggerReason,
                            out stateStatus,
                            out stateStatusReason,
                            out detourDataAgeMilliseconds);

                    if (string.IsNullOrWhiteSpace(
                            stateStatusReason) &&
                        !string.IsNullOrWhiteSpace(
                            LastFailureReason))
                    {
                        stateStatus = "Unavailable";
                        stateStatusReason = LastFailureReason;
                    }

                    return hasDiagnostics;
                }

                jumpCandidateActive = false;
                jumpCandidateConsistentFrameCount = 0;
                estimatedShiftDistanceMeters = 0.0;
                estimatedShiftHeadingRadians = 0.0;
                automaticFrameShiftCount = 0;
                sourceFrameIntervalSeconds = 0.0;
                motionPredictionTimestampSeconds = 0.0;
                hasInnovationDiagnostics = false;
                positionInnovationMeters = 0.0;
                allowedPositionInnovationMeters = 0.0;
                headingInnovationRadians = 0.0;
                allowedHeadingInnovationRadians = 0.0;
                jumpCandidateTriggerReason = string.Empty;
                stateStatus = "Unavailable";
                stateStatusReason = LastFailureReason;
                detourDataAgeMilliseconds = 0.0;
                return false;
            }
        }

        /// <summary>
        /// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
        /// </summary>
        public void Reset()
        {
            lock (_syncRoot)
            {
                if (_poseProvider is DetourVehicleStateProvider
                        detourStateProvider)
                {
                    detourStateProvider.Reset();
                }

                _longitudinalSpeedFilter.Reset();
                _lateralSpeedFilter.Reset();
                _angularSpeedFilter.Reset();
                _wheelSpeedClock.Restart();
                _hasPreviousTimestamp = false;
                _previousTimestampSeconds = 0.0;
                _hasVelocityDiagnostics = false;
                _latestDetourBodyVxMetersPerSecond = 0.0;
                _latestDetourVelocityValid = false;
                _latestRawWheelBodyVxMetersPerSecond = 0.0;
                _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
                _latestRawWheelBodyVyMetersPerSecond = 0.0;
                _latestFilteredWheelBodyVyMetersPerSecond = 0.0;
                _latestRawWheelBodyOmegaRadiansPerSecond = 0.0;
                _latestFilteredWheelBodyOmegaRadiansPerSecond = 0.0;
                _latestWheelSampleTimestampSeconds = 0.0;
                _latestWheelVelocityValid = false;
                _latestWheelFeedbackReadSucceeded = false;
                LastFailureReason = string.Empty;
                LastHeadingFailureReason = string.Empty;
            }
        }

        /// <summary>
        /// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。
        /// </summary>
        private void ReadFilteredWheelTwist(
            out Twist2D filteredTwistInBody,
            out double sampleTimestampSeconds,
            out bool hasValidWheelSpeedEstimate)
        {
            var actualCarSpeed =
                _chassis.GetCarSpeed(true);
            var rawBodyVxMetersPerSecond =
                (double)actualCarSpeed.Vx;
            var rawBodyVyMetersPerSecond =
                (double)actualCarSpeed.Vy;
            // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
            // 状态估计内部统一转换为rad/s。
            var rawBodyOmegaRadiansPerSecond =
                AngleMath.DegreesToRadians(
                    actualCarSpeed.Vw);

            NumericGuard.EnsureFinite(
                rawBodyVxMetersPerSecond,
                "电机反馈车体纵向速度");
            NumericGuard.EnsureFinite(
                rawBodyVyMetersPerSecond,
                "电机反馈车体横向速度");
            NumericGuard.EnsureFinite(
                rawBodyOmegaRadiansPerSecond,
                "电机反馈车体角速度");

            sampleTimestampSeconds =
                _wheelSpeedClock.Elapsed.TotalSeconds;

            UpdateBodyVelocityFilters(
                rawBodyVxMetersPerSecond,
                rawBodyVyMetersPerSecond,
                rawBodyOmegaRadiansPerSecond,
                sampleTimestampSeconds,
                out var filteredBodyVxMetersPerSecond,
                out var filteredBodyVyMetersPerSecond,
                out var filteredBodyOmegaRadiansPerSecond,
                out hasValidWheelSpeedEstimate);

            filteredTwistInBody = new Twist2D(
                filteredBodyVxMetersPerSecond,
                filteredBodyVyMetersPerSecond,
                filteredBodyOmegaRadiansPerSecond);

            _latestRawWheelBodyVxMetersPerSecond =
                rawBodyVxMetersPerSecond;
            _latestFilteredWheelBodyVxMetersPerSecond =
                filteredBodyVxMetersPerSecond;
            _latestRawWheelBodyVyMetersPerSecond =
                rawBodyVyMetersPerSecond;
            _latestFilteredWheelBodyVyMetersPerSecond =
                filteredBodyVyMetersPerSecond;
            _latestRawWheelBodyOmegaRadiansPerSecond =
                rawBodyOmegaRadiansPerSecond;
            _latestFilteredWheelBodyOmegaRadiansPerSecond =
                filteredBodyOmegaRadiansPerSecond;
            _latestWheelSampleTimestampSeconds =
                sampleTimestampSeconds;
            _latestWheelVelocityValid =
                hasValidWheelSpeedEstimate;
            _latestWheelFeedbackReadSucceeded = true;
        }

        /// <summary>
        /// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
        /// </summary>
        private void UpdateBodyVelocityFilters(
            double rawBodyVxMetersPerSecond,
            double rawBodyVyMetersPerSecond,
            double rawBodyOmegaRadiansPerSecond,
            double timestampSeconds,
            out double filteredBodyVxMetersPerSecond,
            out double filteredBodyVyMetersPerSecond,
            out double filteredBodyOmegaRadiansPerSecond,
            out bool hasValidWheelSpeedEstimate)
        {
            NumericGuard.EnsureFiniteNonNegative(
                timestampSeconds,
                nameof(timestampSeconds));

            if (!_hasPreviousTimestamp)
            {
                _longitudinalSpeedFilter.Reset(
                    rawBodyVxMetersPerSecond);
                _lateralSpeedFilter.Reset(
                    rawBodyVyMetersPerSecond);
                _angularSpeedFilter.Reset(
                    rawBodyOmegaRadiansPerSecond);
                _previousTimestampSeconds = timestampSeconds;
                _hasPreviousTimestamp = true;
                hasValidWheelSpeedEstimate = false;
                filteredBodyVxMetersPerSecond =
                    rawBodyVxMetersPerSecond;
                filteredBodyVyMetersPerSecond =
                    rawBodyVyMetersPerSecond;
                filteredBodyOmegaRadiansPerSecond =
                    rawBodyOmegaRadiansPerSecond;
                return;
            }

            var deltaTimeSeconds =
                timestampSeconds -
                _previousTimestampSeconds;
            _previousTimestampSeconds = timestampSeconds;

            if (deltaTimeSeconds <= 0.0)
            {
                _longitudinalSpeedFilter.Reset(
                    rawBodyVxMetersPerSecond);
                _lateralSpeedFilter.Reset(
                    rawBodyVyMetersPerSecond);
                _angularSpeedFilter.Reset(
                    rawBodyOmegaRadiansPerSecond);
                hasValidWheelSpeedEstimate = false;
                filteredBodyVxMetersPerSecond =
                    rawBodyVxMetersPerSecond;
                filteredBodyVyMetersPerSecond =
                    rawBodyVyMetersPerSecond;
                filteredBodyOmegaRadiansPerSecond =
                    rawBodyOmegaRadiansPerSecond;
                return;
            }

            hasValidWheelSpeedEstimate = true;
            filteredBodyVxMetersPerSecond =
                _longitudinalSpeedFilter.Update(
                    rawBodyVxMetersPerSecond,
                    deltaTimeSeconds);
            filteredBodyVyMetersPerSecond =
                _lateralSpeedFilter.Update(
                    rawBodyVyMetersPerSecond,
                    deltaTimeSeconds);
            filteredBodyOmegaRadiansPerSecond =
                _angularSpeedFilter.Update(
                    rawBodyOmegaRadiansPerSecond,
                    deltaTimeSeconds);
        }

    }
}