ParkingRobot/MultiWheelC/Control/Execution/ParkingGeometricController.cs

using System;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Control.Execution
{
    /// <summary>
    /// 表示新版停车机器人单周期轨迹控制的执行结果。
    /// </summary>
    public enum ParkingControlCycleResult
    {
        Inactive = 0,
        CommandSent = 1,
        Completed = 2,
        StateUnavailable = 3,
        Faulted = 4
    }

    /// <summary>
    /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
    /// </summary>
    public sealed class ParkingGeometricController
    {
        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
            1e-6;
        private const double StartupRegionMeters = 0.02;
        private const double StartupPreviewDistanceMeters = 0.05;
        private const double MaximumStartupSpeedMetersPerSecond = 0.05;

        private readonly IVehicleStateProvider _stateProvider;
        private readonly ILateralController _lateralController;
        private readonly ILongitudinalController _longitudinalController;
        private readonly AckermannGcpAllocator _gcpAllocator;
        private readonly GcpCommandExecutor _commandExecutor;

        private Trajectory2D _trajectory;

        /// <summary>
        /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
        /// </summary>
        public ParkingGeometricController(
            IVehicleStateProvider stateProvider,
            ILateralController lateralController,
            ILongitudinalController longitudinalController,
            AckermannGcpAllocator gcpAllocator,
            GcpCommandExecutor commandExecutor,
            double finishDistanceMeters = 0.03,
            double finishSpeedMetersPerSecond = 0.02,
            double finishHeadingToleranceRadians =
                3.0 * Math.PI / 180.0,
            double maximumDistanceToTrajectoryMeters = 0.50)
        {
            _stateProvider = stateProvider ??
                throw new ArgumentNullException(
                    nameof(stateProvider));
            _lateralController = lateralController ??
                throw new ArgumentNullException(
                    nameof(lateralController));
            _longitudinalController = longitudinalController ??
                throw new ArgumentNullException(
                    nameof(longitudinalController));
            _gcpAllocator = gcpAllocator ??
                throw new ArgumentNullException(
                    nameof(gcpAllocator));
            _commandExecutor = commandExecutor ??
                throw new ArgumentNullException(
                    nameof(commandExecutor));

            EnsureFinitePositive(
                finishDistanceMeters,
                nameof(finishDistanceMeters));
            EnsureFiniteNonNegative(
                finishSpeedMetersPerSecond,
                nameof(finishSpeedMetersPerSecond));
            EnsureFinitePositive(
                finishHeadingToleranceRadians,
                nameof(finishHeadingToleranceRadians));
            EnsureFinitePositive(
                maximumDistanceToTrajectoryMeters,
                nameof(maximumDistanceToTrajectoryMeters));

            FinishDistanceMeters = finishDistanceMeters;
            FinishSpeedMetersPerSecond =
                finishSpeedMetersPerSecond;
            FinishHeadingToleranceRadians =
                finishHeadingToleranceRadians;
            MaximumDistanceToTrajectoryMeters =
                maximumDistanceToTrajectoryMeters;
        }

        /// <summary>
        /// 获取终点位置和剩余弧长允许的误差，单位为m。
        /// </summary>
        public double FinishDistanceMeters { get; }

        /// <summary>
        /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
        /// </summary>
        public double FinishSpeedMetersPerSecond { get; }

        /// <summary>
        /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
        /// </summary>
        public double FinishHeadingToleranceRadians { get; }

        /// <summary>
        /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
        /// </summary>
        public double MaximumDistanceToTrajectoryMeters { get; }

        /// <summary>
        /// 获取控制器当前是否持有并正在执行一条轨迹。
        /// </summary>
        public bool IsActive { get; private set; }

        /// <summary>
        /// 获取最近一次轨迹是否已经满足终点完成条件。
        /// </summary>
        public bool IsCompleted { get; private set; }

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

        /// <summary>
        /// 获取最近一次控制异常，正常时为空。
        /// </summary>
        public Exception LastException { get; private set; }

        /// <summary>
        /// 获取最近一次有效车辆状态。
        /// </summary>
        public VehicleState? LastVehicleState { get; private set; }

        /// <summary>
        /// 获取最近一次车体中心到参考轨迹的投影结果。
        /// </summary>
        public TrajectoryProjection? LastProjection { get; private set; }

        /// <summary>
        /// 获取最近一次发送或准备发送的GCP运动命令。
        /// </summary>
        public GcpMotionCommand? LastCommand { get; private set; }

        /// <summary>
        /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
        /// </summary>
        public double? LastReferenceSpeedMetersPerSecond { get; private set; }

        /// <summary>
        /// 停止当前底盘并从起点开始执行指定二维轨迹。
        /// </summary>
        public void Start(Trajectory2D trajectory)
        {
            if (trajectory == null)
            {
                throw new ArgumentNullException(
                    nameof(trajectory));
            }

            StopAndResetControllers();
            _trajectory = trajectory;
            IsActive = true;
            IsCompleted = false;
            ClearDiagnostics();
        }

        /// <summary>
        /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
        /// </summary>
        public ParkingControlCycleResult ExecuteCycle(
            double deltaTimeSeconds)
        {
            EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (!IsActive || _trajectory == null)
            {
                return ParkingControlCycleResult.Inactive;
            }

            try
            {
                if (!_stateProvider.TryGetState(
                        out var vehicleState))
                {
                    StopForUnavailableState();
                    return ParkingControlCycleResult
                        .StateUnavailable;
                }

                LastVehicleState = vehicleState;

                var projection = TrajectoryProjector.Project(
                    _trajectory,
                    vehicleState.PoseInWorld);
                LastProjection = projection;

                if (projection.DistanceToTrajectoryMeters >
                    MaximumDistanceToTrajectoryMeters)
                {
                    return EnterFault(
                        "车辆距离参考轨迹" +
                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
                        "超过允许值" +
                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
                }

                if (HasReachedEnd(
                        vehicleState,
                        projection))
                {
                    CompleteTrajectory();
                    return ParkingControlCycleResult.Completed;
                }

                if (HasStoppedAtUnsatisfiedTerminal(
                        vehicleState,
                        projection,
                        out var terminalFailureReason))
                {
                    return EnterFault(
                        terminalFailureReason);
                }

                var referenceSpeedMetersPerSecond =
                    ResolveReferenceSpeedForControl(
                        projection);
                LastReferenceSpeedMetersPerSecond =
                    referenceSpeedMetersPerSecond;
                var context = new PathTrackingContext(
                    vehicleState,
                    projection,
                    referenceSpeedMetersPerSecond,
                    deltaTimeSeconds);
                var lateralCommand =
                    _lateralController.Compute(context);
                var commandSpeedMetersPerSecond =
                    _longitudinalController
                        .ComputeSpeedMetersPerSecond(context);
                var gcpCommand = _gcpAllocator.Allocate(
                    commandSpeedMetersPerSecond,
                    lateralCommand);

                if (!_commandExecutor.Execute(
                        gcpCommand,
                        deltaTimeSeconds))
                {
                    return EnterFault(
                        string.IsNullOrWhiteSpace(
                            _commandExecutor.LastFailureReason)
                            ? "GCP底盘命令执行失败。"
                            : _commandExecutor.LastFailureReason);
                }

                LastCommand =
                    _commandExecutor.LastSentCommand;

                LastFailureReason = string.Empty;
                LastException = null;
                return ParkingControlCycleResult.CommandSent;
            }
            catch (Exception exception)
            {
                return EnterFault(
                    "停车机器人轨迹控制周期异常：" +
                    exception.Message,
                    exception);
            }
        }

        /// <summary>
        /// 主动取消当前轨迹、立即停车并清除全部控制器状态。
        /// </summary>
        public void Cancel()
        {
            StopAndResetControllers();
            _trajectory = null;
            IsActive = false;
            IsCompleted = false;
            ClearDiagnostics();
        }

        /// <summary>
        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
        /// </summary>
        private double ResolveReferenceSpeedForControl(
            TrajectoryProjection projection)
        {
            var currentReferenceSpeed =
                projection.ReferencePoint
                    .ReferenceSpeedMetersPerSecond;

            var requiresStartupRelease =
                projection.ArcLengthMeters <=
                    StartupRegionMeters &&
                projection.RemainingDistanceMeters >
                    FinishDistanceMeters &&
                Math.Abs(currentReferenceSpeed) <=
                    ZeroReferenceSpeedToleranceMetersPerSecond;

            if (!requiresStartupRelease)
            {
                return currentReferenceSpeed;
            }

            var previewArcLengthMeters = Math.Min(
                _trajectory.TotalLengthMeters,
                projection.ArcLengthMeters +
                StartupPreviewDistanceMeters);
            var previewReferenceSpeed =
                _trajectory
                    .SampleAtArcLength(
                        previewArcLengthMeters)
                    .ReferenceSpeedMetersPerSecond;

            if (Math.Abs(previewReferenceSpeed) <=
                ZeroReferenceSpeedToleranceMetersPerSecond)
            {
                return 0.0;
            }

            return Math.Sign(previewReferenceSpeed) *
                   Math.Min(
                       Math.Abs(previewReferenceSpeed),
                       MaximumStartupSpeedMetersPerSecond);
        }

        /// <summary>
        /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
        /// </summary>
        private bool HasReachedEnd(
            VehicleState vehicleState,
            TrajectoryProjection projection)
        {
            if (!vehicleState.HasValidVelocityEstimate)
            {
                return false;
            }

            return projection.RemainingDistanceMeters <=
                       FinishDistanceMeters &&
                   CalculateDistanceToEndMeters(
                       vehicleState) <=
                       FinishDistanceMeters &&
                   CalculateHeadingErrorToEndRadians(
                       vehicleState) <=
                       FinishHeadingToleranceRadians &&
                   CalculateActualLinearSpeedMetersPerSecond(
                       vehicleState) <=
                       FinishSpeedMetersPerSecond;
        }

        /// <summary>
        /// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
        /// </summary>
        private bool HasStoppedAtUnsatisfiedTerminal(
            VehicleState vehicleState,
            TrajectoryProjection projection,
            out string failureReason)
        {
            failureReason = string.Empty;

            var isTerminalZeroSpeedReference =
                projection.RemainingDistanceMeters <=
                    FinishDistanceMeters &&
                Math.Abs(
                    projection.ReferencePoint
                        .ReferenceSpeedMetersPerSecond) <=
                    ZeroReferenceSpeedToleranceMetersPerSecond;

            if (!isTerminalZeroSpeedReference ||
                !vehicleState.HasValidVelocityEstimate ||
                CalculateActualLinearSpeedMetersPerSecond(
                    vehicleState) >
                FinishSpeedMetersPerSecond)
            {
                return false;
            }

            var positionErrorMeters =
                CalculateDistanceToEndMeters(
                    vehicleState);
            var headingErrorRadians =
                CalculateHeadingErrorToEndRadians(
                    vehicleState);

            failureReason =
                "车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
                $"位置误差={positionErrorMeters:F3}m，" +
                "航向误差=" +
                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
            return true;
        }

        /// <summary>
        /// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
        /// </summary>
        private double CalculateDistanceToEndMeters(
            VehicleState vehicleState)
        {
            var endPoint = _trajectory.EndPoint.PoseInWorld;
            var deltaX =
                vehicleState.PoseInWorld.XMeters -
                endPoint.XMeters;
            var deltaY =
                vehicleState.PoseInWorld.YMeters -
                endPoint.YMeters;

            return Math.Sqrt(
                deltaX * deltaX +
                deltaY * deltaY);
        }

        /// <summary>
        /// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
        /// </summary>
        private double CalculateHeadingErrorToEndRadians(
            VehicleState vehicleState)
        {
            return Math.Abs(
                AngleMath.ShortestDifferenceRadians(
                    _trajectory.EndPoint
                        .PoseInWorld.YawRadians,
                    vehicleState
                        .PoseInWorld.YawRadians));
        }

        /// <summary>
        /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
        /// </summary>
        private static double CalculateActualLinearSpeedMetersPerSecond(
            VehicleState vehicleState)
        {
            return Math.Sqrt(
                vehicleState.TwistInBody.VxMetersPerSecond *
                vehicleState.TwistInBody.VxMetersPerSecond +
                vehicleState.TwistInBody.VyMetersPerSecond *
                vehicleState.TwistInBody.VyMetersPerSecond);
        }

        /// <summary>
        /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
        /// </summary>
        private void StopForUnavailableState()
        {
            _commandExecutor.Stop();
            _lateralController.Reset();
            _longitudinalController.Reset();
            LastCommand = null;
            LastFailureReason =
                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
            LastException = null;
        }

        /// <summary>
        /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
        /// </summary>
        private void CompleteTrajectory()
        {
            StopAndResetControllers();
            IsActive = false;
            IsCompleted = true;
            LastCommand = new GcpMotionCommand(
                0.0,
                0.0,
                0.0);
            LastFailureReason = string.Empty;
            LastException = null;
        }

        /// <summary>
        /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
        /// </summary>
        private ParkingControlCycleResult EnterFault(
            string reason,
            Exception exception = null)
        {
            StopAndResetControllers();
            IsActive = false;
            IsCompleted = false;
            LastCommand = null;
            LastFailureReason = reason;
            LastException = exception;
            return ParkingControlCycleResult.Faulted;
        }

        /// <summary>
        /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
        /// </summary>
        private void StopAndResetControllers()
        {
            _commandExecutor.Stop();
            _lateralController.Reset();
            _longitudinalController.Reset();
        }

        /// <summary>
        /// 清除上一条轨迹留下的状态、命令和故障诊断信息。
        /// </summary>
        private void ClearDiagnostics()
        {
            LastVehicleState = null;
            LastProjection = null;
            LastCommand = null;
            LastReferenceSpeedMetersPerSecond = null;
            LastFailureReason = string.Empty;
            LastException = null;
        }

        /// <summary>
        /// 检查控制参数是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value) ||
                value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹控制器距离和周期参数必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查控制参数是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value) ||
                value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹控制器速度参数必须是非负有限值。");
            }
        }
    }
}