打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -1 +1,177 @@
-// 负责把纯数学命令转换成现有底盘调用
+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 double _lastFrontAngleRadians;
+        private double _lastRearAngleRadians;
+
+        /// <summary>
+        /// 创建绑定指定单车底盘适配器的GCP命令执行器。
+        /// </summary>
+        public GcpCommandExecutor(
+            MultiWheelChassisAdapter chassisAdapter,
+            double maximumGcpAngleRateRadiansPerSecond =
+                10.0 * Math.PI / 180.0)
+        {
+            _chassisAdapter = chassisAdapter ??
+                throw new ArgumentNullException(
+                    nameof(chassisAdapter));
+            EnsureFinitePositive(
+                maximumGcpAngleRateRadiansPerSecond,
+                nameof(maximumGcpAngleRateRadiansPerSecond));
+
+            MaximumGcpAngleRateRadiansPerSecond =
+                maximumGcpAngleRateRadiansPerSecond;
+        }
+
+        /// <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 success = _chassisAdapter.SendGcpMotion(
+                limitedCommand.SpeedMetersPerSecond,
+                limitedCommand.FrontAngleRadians,
+                limitedCommand.RearAngleRadians,
+                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可表示的正有限秒数。");
+            }
+        }
+    }
+}