拆分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,103 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    internal static class MovementTestPreparation
+    {
+        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
+        public static bool AlignWheelsForward(
+            ref DriveTask activeTask)
+        {
+            var preparation = new PrepareWheelsForward();
+            var task = new DriveTask(preparation.Get());
+            activeTask = task;
+
+            try
+            {
+                task.Wait();
+                return preparation.Completed;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"测试前舵轮回正失败：{ex.Message}");
+                return false;
+            }
+            finally
+            {
+                task.Stop();
+                if (ReferenceEquals(activeTask, task))
+                    activeTask = null;
+            }
+        }
+
+        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
+        public static bool AreWheelsForward(
+            float toleranceDegrees = 2f)
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
+                return false;
+            }
+
+            try
+            {
+                var adapter = new MultiWheelChassisAdapter(
+                    chassis,
+                    PilotDefinition.Self.CarNum);
+                var toleranceRadians =
+                    AngleMath.DegreesToRadians(toleranceDegrees);
+
+                if (adapter.AreParallelWheelsAligned(
+                        0.0,
+                        toleranceRadians))
+                {
+                    return true;
+                }
+
+                Console.WriteLine(
+                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
+                return false;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"检查舵轮方向失败：{ex.Message}");
+                return false;
+            }
+        }
+    }
+
+    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
+    public class AlignWheelsForwardTest : MovementTest
+    {
+        private DriveTask _task;
+
+        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
+        public override void Test()
+        {
+            MovementTestPreparation.AlignWheelsForward(
+                ref _task);
+        }
+
+        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+        }
+    }
+}