ParkingRobot/MultiWheelC/Fleet/FleetController.cs

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

namespace MultiWheelC.Fleet
{
    // 表示车队中心单周期轨迹控制的计算结果，不包含通信发送结果。
    public enum FleetControlCycleResult
    {
        Inactive = 0,
        CommandGenerated = 1,
        Completed = 2,
        Faulted = 3
    }

    // 将公共轨迹核心生成的GCP命令转换为车队坐标系下的刚体速度命令。
    public sealed class FleetController
    {
        private readonly PathTrackingCore _trackingCore;

        // virtualControlPointRadiusMeters必须与横向控制器采用的虚拟车队GCP半径一致。
        public FleetController(
            ILateralController lateralController,
            ILongitudinalController longitudinalController,
            GcpCommandAllocator gcpAllocator,
            double virtualControlPointRadiusMeters,
            double finishDistanceMeters = 0.04,
            double finishSpeedMetersPerSecond = 0.02,
            double finishHeadingToleranceRadians =
                3.0 * Math.PI / 180.0,
            double maximumDistanceToTrajectoryMeters = 0.30,
            double terminalBrakingPreviewMeters = 0.02,
            double terminalApproachDistanceMeters = 0.10,
            double terminalApproachGainPerSecond = 0.8,
            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
            double curvaturePreviewSeconds = 0.20,
            double maximumCurvaturePreviewMeters = 0.12,
            double motionDirectionInFleetRadians = 0.0)
        {
            NumericGuard.EnsureFinitePositive(
                virtualControlPointRadiusMeters,
                nameof(virtualControlPointRadiusMeters));
            NumericGuard.EnsureFinite(
                motionDirectionInFleetRadians,
                nameof(motionDirectionInFleetRadians));

            VirtualControlPointRadiusMeters =
                virtualControlPointRadiusMeters;
            MotionDirectionInFleetRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionInFleetRadians);
            _trackingCore = new PathTrackingCore(
                lateralController,
                longitudinalController,
                gcpAllocator,
                finishDistanceMeters,
                finishSpeedMetersPerSecond,
                finishHeadingToleranceRadians,
                maximumDistanceToTrajectoryMeters,
                terminalBrakingPreviewMeters,
                terminalApproachDistanceMeters,
                terminalApproachGainPerSecond,
                maximumTerminalApproachSpeedMetersPerSecond,
                curvaturePreviewSeconds,
                maximumCurvaturePreviewMeters,
                MotionDirectionInFleetRadians);
        }

        // 虚拟车队中心到前、后GCP的距离，单位为m。
        public double VirtualControlPointRadiusMeters { get; }

        // 当前运动坐标系+X轴相对车队坐标系+X轴的方向，单位为rad。
        public double MotionDirectionInFleetRadians { get; }

        public double FinishDistanceMeters =>
            _trackingCore.FinishDistanceMeters;

        public double FinishSpeedMetersPerSecond =>
            _trackingCore.FinishSpeedMetersPerSecond;

        public double FinishHeadingToleranceRadians =>
            _trackingCore.FinishHeadingToleranceRadians;

        public double MaximumDistanceToTrajectoryMeters =>
            _trackingCore.MaximumDistanceToTrajectoryMeters;

        public double TerminalBrakingPreviewMeters =>
            _trackingCore.TerminalBrakingPreviewMeters;

        public double TerminalApproachDistanceMeters =>
            _trackingCore.TerminalApproachDistanceMeters;

        public double TerminalApproachGainPerSecond =>
            _trackingCore.TerminalApproachGainPerSecond;

        public double MaximumTerminalApproachSpeedMetersPerSecond =>
            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;

        public double CurvaturePreviewSeconds =>
            _trackingCore.CurvaturePreviewSeconds;

        public double MaximumCurvaturePreviewMeters =>
            _trackingCore.MaximumCurvaturePreviewMeters;

        public bool IsActive => _trackingCore.IsActive;

        public bool IsCompleted => _trackingCore.IsCompleted;

        public string LastFailureReason =>
            _trackingCore.LastFailureReason;

        public Exception LastException =>
            _trackingCore.LastException;

        public FleetState? LastFleetState { get; private set; }

        public TrajectoryProjection? LastProjection =>
            _trackingCore.LastProjection;

        public GcpMotionCommand? LastGcpCommand =>
            _trackingCore.LastRequestedCommand;

        public FleetMotionCommand? LastCommand { get; private set; }

        // 重置公共核心，并从轨迹起点开始跟踪虚拟车队中心。
        public void Start(Trajectory2D trajectory)
        {
            _trackingCore.Start(trajectory);
            LastFleetState = null;
            LastCommand = null;
        }

        // 计算一周期车队中心命令；非运行状态、完成或故障时返回停车命令。
        public FleetControlCycleResult ComputeCommand(
            FleetState fleetState,
            double deltaTimeSeconds,
            out FleetMotionCommand command)
        {
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            command = FleetMotionCommand.Stop();

            if (!_trackingCore.IsActive)
            {
                return FleetControlCycleResult.Inactive;
            }

            LastFleetState = fleetState;
            var output = _trackingCore.Compute(
                fleetState.FleetPoseInWorld,
                fleetState.TwistAtFleetOriginInFleet,
                fleetState.HasValidVelocityEstimate,
                deltaTimeSeconds);

            if (output.Result ==
                PathTrackingCycleResult.Completed)
            {
                LastCommand = command;
                return FleetControlCycleResult.Completed;
            }

            if (output.Result ==
                PathTrackingCycleResult.Faulted)
            {
                LastCommand = command;
                return FleetControlCycleResult.Faulted;
            }

            if (output.Result !=
                    PathTrackingCycleResult.CommandGenerated ||
                !output.Command.HasValue)
            {
                return FleetControlCycleResult.Inactive;
            }

            try
            {
                var twistAtFleetOriginInMotionFrame =
                    GcpKinematics.ToBodyTwist(
                        output.Command.Value,
                        VirtualControlPointRadiusMeters);
                var twistAtFleetOriginInFleet =
                    FrameTransform2D.TransformTwistAtSamePoint(
                        new Pose2D(
                            0.0,
                            0.0,
                            MotionDirectionInFleetRadians),
                        twistAtFleetOriginInMotionFrame);
                command = new FleetMotionCommand(
                    Point2D.Zero,
                    twistAtFleetOriginInFleet);
                LastCommand = command;
                return FleetControlCycleResult.CommandGenerated;
            }
            catch (Exception exception)
            {
                _trackingCore.Fail(
                    "车队中心GCP命令转换异常：" +
                    exception.Message,
                    exception);
                LastCommand = command;
                return FleetControlCycleResult.Faulted;
            }
        }

        // 取消当前轨迹并使后续周期只生成停车命令。
        public void Cancel()
        {
            _trackingCore.Cancel();
            LastFleetState = null;
            LastCommand = null;
        }
    }
}