拆分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,55 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Clumsy.Tracks;
+
+namespace MultiWheelC
+{
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        // 本次轨迹的巡航速度上限，单位m/s。
+        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+        public override IEnumerable<bool> Get()
+        {
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            DriveTask task = null;
+            try
+            {
+                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+                var tracker = new ChassisController
+                {
+                    BaseSpeed = MaxSpeed
+                }.Get();
+                // 要求路径末端速度下降到零。
+                tracker.FinishSpeed = 0f;
+                var linePath = new LineTrack(Src, Dst)
+                {
+                    CarDirectionBias = CarDirectionBias,
+                    Speed = MaxSpeed
+                };
+                tracker.AddTrack(linePath);
+                task = new DriveTask(tracker.Track());
+                task.Wait();
+                yield return false;
+            }
+            finally
+            {
+                task?.Stop();
+                chassis.PredefinedDriveStop();
+            }
+        }
+
+    }
+}