拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 18:07:01 +08:00
parent 7447317812
commit 47973cc94b
38 changed files with 3982 additions and 976 deletions
@@ -0,0 +1,92 @@
+
+
+
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+    public class PrepareWheelsForward : MovementDefinition
+    {
+        public float ToleranceDegrees = 2f;
+        public float StableSeconds = 0.3f;
+        public float TimeoutSeconds = 10f;
+        public bool Completed { get; private set; }
+
+        public override IEnumerable<bool> Get()
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+            adapter.ResetToBodyFrame();
+            var toleranceRadians =
+                AngleMath.DegreesToRadians(ToleranceDegrees);
+            var startTime = DateTime.UtcNow;
+            DateTime? alignedSince = null;
+
+            Completed = false;
+            if (!adapter.PrepareParallelDirection(0.0))
+            {
+                throw new InvalidOperationException(
+                    "无法将所有舵轮下发到车体前向0°。");
+            }
+
+            try
+            {
+                while (true)
+                {
+                    var aligned =
+                        adapter.AreParallelWheelsAligned(
+                            0.0,
+                            toleranceRadians);
+
+                    if (aligned)
+                    {
+                        if (!alignedSince.HasValue)
+                            alignedSince = DateTime.UtcNow;
+
+                        if ((DateTime.UtcNow -
+                                alignedSince.Value).TotalSeconds >=
+                            StableSeconds)
+                        {
+                            Completed = true;
+                            yield break;
+                        }
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+                            "测试已经取消。");
+                    }
+
+                    yield return true;
+                }
+            }
+            finally
+            {
+                // 只清零驱动速度，保留已经下发的0°舵角。
+                adapter.StopImmediately();
+            }
+        }
+    }
+}