ParkingRobot/MultiWheelC/Control/Execution/GcpCommandExecutor.cs

using System;
using MultiWheelC.Control.Allocation;
using MyParking.Shared;

namespace MultiWheelC.Control.Execution
{
    /// <summary>
    /// 将SI单位的GCP运动命令安全转换为现有多舵轮底盘调用。
    /// </summary>
    public sealed class GcpCommandExecutor
    {
        private const double StopSpeedDeadbandMetersPerSecond =
            1e-6;

        private readonly MultiWheelChassisAdapter _chassisAdapter;
        private readonly double _motionDirectionInBodyRadians;
        private double _lastFrontAngleRadians;
        private double _lastRearAngleRadians;

        /// <summary>
        /// 创建绑定指定单车底盘适配器的GCP命令执行器。
        /// </summary>
        public GcpCommandExecutor(
            MultiWheelChassisAdapter chassisAdapter,
            double maximumGcpAngleRateRadiansPerSecond =
                10.0 * Math.PI / 180.0,
            double motionDirectionInBodyRadians = 0.0)
        {
            _chassisAdapter = chassisAdapter ??
                throw new ArgumentNullException(
                    nameof(chassisAdapter));
            EnsureFinitePositive(
                maximumGcpAngleRateRadiansPerSecond,
                nameof(maximumGcpAngleRateRadiansPerSecond));
            NumericGuard.EnsureFinite(
                motionDirectionInBodyRadians,
                nameof(motionDirectionInBodyRadians));

            MaximumGcpAngleRateRadiansPerSecond =
                maximumGcpAngleRateRadiansPerSecond;
            _motionDirectionInBodyRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionInBodyRadians);
        }

        /// <summary>
        /// 获取执行器绑定的车辆编号。
        /// </summary>
        public int VehicleId =>
            _chassisAdapter.VehicleId;

        /// <summary>
        /// 获取前后GCP目标角度允许的最大变化率，单位为rad/s。
        /// </summary>
        public double MaximumGcpAngleRateRadiansPerSecond { get; }

        /// <summary>
        /// 获取最近一次控制器请求的未限速GCP命令。
        /// </summary>
        public GcpMotionCommand? LastRequestedCommand { get; private set; }

        /// <summary>
        /// 获取最近一次经过GCP角速度限制后实际发送给底盘的命令。
        /// </summary>
        public GcpMotionCommand? LastSentCommand { get; private set; }

        /// <summary>
        /// 获取最近一次旧版底盘运动分解失败原因。
        /// </summary>
        public string LastFailureReason { get; private set; } =
            string.Empty;

        /// <summary>
        /// 使用真实控制周期执行一条GCP命令，并在分解失败时保持停车。
        /// </summary>
        public bool Execute(
            GcpMotionCommand command,
            double deltaTimeSeconds)
        {
            EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));
            LastRequestedCommand = command;

            if (Math.Abs(command.SpeedMetersPerSecond) <=
                StopSpeedDeadbandMetersPerSecond)
            {
                Stop();
                LastSentCommand = new GcpMotionCommand(
                    0.0,
                    _lastFrontAngleRadians,
                    _lastRearAngleRadians);
                return true;
            }

            var maximumAngleChangeRadians =
                MaximumGcpAngleRateRadiansPerSecond *
                deltaTimeSeconds;
            _lastFrontAngleRadians = MoveTowards(
                _lastFrontAngleRadians,
                command.FrontAngleRadians,
                maximumAngleChangeRadians);
            _lastRearAngleRadians = MoveTowards(
                _lastRearAngleRadians,
                command.RearAngleRadians,
                maximumAngleChangeRadians);

            var limitedCommand = new GcpMotionCommand(
                command.SpeedMetersPerSecond,
                _lastFrontAngleRadians,
                _lastRearAngleRadians);
            LastSentCommand = limitedCommand;

            var motionFrameTwist =
                GcpKinematics.ToBodyTwist(
                    limitedCommand,
                    _chassisAdapter.ControlPointRadiusMeters);
            var bodyTwist =
                FrameTransform2D.TransformTwistAtSamePoint(
                    new Pose2D(
                        0.0,
                        0.0,
                        _motionDirectionInBodyRadians),
                    motionFrameTwist);
            var success = _chassisAdapter.SendBodyTwist(
                bodyTwist,
                TimeSpan.FromSeconds(deltaTimeSeconds));

            LastFailureReason = success
                ? string.Empty
                : BuildFailureReason();

            return success;
        }

        /// <summary>
        /// 立即清零底盘驱动速度并清除执行器失败状态。
        /// </summary>
        public void Stop()
        {
            _chassisAdapter.StopImmediately();
            LastFailureReason = string.Empty;
        }

        /// <summary>
        /// 以不超过指定单周期变化量的速度使当前值接近目标值。
        /// </summary>
        private static double MoveTowards(
            double current,
            double target,
            double maximumChange)
        {
            var difference = target - current;

            if (Math.Abs(difference) <= maximumChange)
            {
                return target;
            }

            return current +
                   Math.Sign(difference) *
                   maximumChange;
        }

        /// <summary>
        /// 将底盘返回的空失败原因替换为可诊断的默认说明。
        /// </summary>
        private string BuildFailureReason()
        {
            return string.IsNullOrWhiteSpace(
                    _chassisAdapter.LastFailureReason)
                ? "旧版SendMotion未能完成GCP运动分解。"
                : _chassisAdapter.LastFailureReason;
        }

        /// <summary>
        /// 检查控制周期是否为正有限值且能够转换为TimeSpan。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value) ||
                value <= 0.0 ||
                value > TimeSpan.MaxValue.TotalSeconds)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "GCP命令控制周期必须是TimeSpan可表示的正有限秒数。");
            }
        }
    }
}