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

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -351,6 +351,53 @@ namespace MyParking.Shared
            return success;
        }

+        /// <summary>
+        /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
+        /// </summary>
+        public bool SendGcpMotion(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians,
+            TimeSpan? interval = null)
+        {
+            ValidateFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            ValidateFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            ValidateFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
+            EnsureBodyFrameIsActive();
+
+            if (Math.Abs(frontAngleRadians) >=
+                    Math.PI / 2.0 ||
+                Math.Abs(rearAngleRadians) >=
+                    Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(frontAngleRadians),
+                    "前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。");
+            }
+
+            var success = _chassis.SendMotion(
+                (float)speedMetersPerSecond,
+                (float)(frontAngleRadians *
+                        RadiansToDegrees),
+                (float)(rearAngleRadians *
+                        RadiansToDegrees),
+                interval);
+
+            if (!success)
+            {
+                // 分解失败后立即清除上一条驱动速度，避免车辆继续执行陈旧命令。
+                _chassis.PredefinedDriveStop();
+            }
+
+            return success;
+        }
+
        /// <summary>
        /// 立即将所有驱动轮速度下发为零。
        /// </summary>