增加dstTracker

MultiWheel/MultiWheelC/AGV.cs

@@ -1,5 +1,7 @@
 using ClumsyCore;
 using ClumsyCore.Interfaces;
+using ClumsyCore.Sensors;
+using CommonUsage.Chassis;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
 using MDCSToolBox.Clumsy.MotionControllers;
@@ -10,6 +12,7 @@ using System;
 using System.Collections.Generic;
 using System.Net.Http;
 using System.Numerics;
+using System.Security.Cryptography;
 using System.Threading;
 using System.Threading.Tasks;
 using static ClumsyCore.DTools.Painter;
@@ -65,20 +68,20 @@ namespace MultiWheelC
             }
         }
 
-        //参数1：tireNum 需要钻过的轮胎对数量
+        //参数1：tireNum 需要钻过的轮胎对数量
-        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
+        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
         public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
         {
             while (!TryLock(dstId))
             {
                 Thread.Sleep(50);
             }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
+            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
-            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
+            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
             if (tireNum != 1 && tireNum != 2)
             {
-                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
+                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
                 return;
             }
 
@@ -86,20 +89,20 @@ namespace MultiWheelC
             {
                 while (!TryLock(dstId))
                 {
-                    Console.WriteLine("等待锁取货点中...");
+                    Console.WriteLine("等待锁取货点中...");
                     Thread.Sleep(200);
                 }
-                Console.WriteLine($"锁点{dstId}完成");
+                Console.WriteLine($"锁点{dstId}完成");
                 Thread.Sleep(1000);
-                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
+                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
                 Thread.Sleep(1000);
                 Leave(srcId);
-                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
+                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
                 Thread.Sleep(2000);
                 //Leave(dstId);
-                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
+                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
                 Thread.Sleep(2000);
-                Console.WriteLine($"结束钻车动作");
+                Console.WriteLine($"结束钻车动作");
                 return;
             }
 
@@ -133,7 +136,7 @@ namespace MultiWheelC
                     0)
                 },
             };
-            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
+            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
             var following = new TireFollowing()
             {
                 GetController = () => new ChassisController().Get(),
@@ -148,18 +151,18 @@ namespace MultiWheelC
             };
             var _dt = new DriveTask(following.Get());
             _dt.Wait();
-            DLog.Log("钻车动作结束", "TireFollowing");
+            DLog.Log("钻车动作结束", "TireFollowing");
         }
 
-        //离车一定是后雷达识别一个轮胎
+        //离车一定是后雷达识别一个轮胎
         public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
         {
             while (!TryLock(dstId))
             {
                 Thread.Sleep(50);
             }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
+            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
 
             var following = new TireFollowing()
             {
@@ -173,7 +176,7 @@ namespace MultiWheelC
                         DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
                         StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
                         StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
                         FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                         PathTransformation = new Tuple<float, float, float>(
                         PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
@@ -190,39 +193,97 @@ namespace MultiWheelC
                 WalkBlindTh = 0,
                 TireNum = 1
             };
-            var _dt = new DriveTask(following.Get());
+
-            _dt.Wait();
+            IEnumerable<bool> LeaveThenFollow()
-            new DriveTask(new DstTracker()
             {
-                Src = new Vector2(srcX, srcY),
+                foreach (var running in following.Get())
-                Dst = new Vector2(dstX, dstY),
+                {
-            }.Get()); Wait();
+                    if (!running) break;
-            DLog.Log("钻车动作结束", "TireFollowing");
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            var _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作1结束", "TireFollowing");
         }
 
-        //驱动器上使能
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            var _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+            //Leave(srcId);
+            DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+            DLog.Log("离车动作2结束", "TireFollowing");
+        }
+
+        //驱动器上使能
         public void DriverAble()
         {
             var dl = new DriveTask(new DriverAble() { }.Get());
             dl.Wait();
-            DLog.Log("驱动器上使能完成", "TireFollowing");
+            DLog.Log("驱动器上使能完成", "TireFollowing");
         }
 
-        //驱动器下使能
+        //驱动器下使能
         public void DriverDisable()
         {
             var dl = new DriveTask(new DriverDisable() { }.Get());
             dl.Wait();
-            DLog.Log("驱动器下使能完成", "TireFollowing");
+            DLog.Log("驱动器下使能完成", "TireFollowing");
         }
 
-        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
+        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
         public void ClamptoTarget(bool close)
         {
             if (PilotDefinition.Self.GhostMode)
             {
                 Thread.Sleep(2000);
-                Console.WriteLine("夹抱完成");
+                Console.WriteLine("夹抱完成");
                 return;
             }
             new DriveTask(new ClampToTarget()
@@ -231,8 +292,8 @@ namespace MultiWheelC
                 RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
             }.Get()).Wait();
         }
-        // 车队联动-自动蟹行：供调度按绝对起终点触发。
+        // 车队联动-自动蟹行：供调度按绝对起终点触发。
-        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
+        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
         public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
             float speed, float CarDirectionBias)
         {
@@ -256,7 +317,7 @@ namespace MultiWheelC
                 {
                     Thread.Sleep(50);
                 }
-                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
+                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
             }
 
             var action = new MultiWheelC.FleetCrabWalk
@@ -286,33 +347,11 @@ namespace MultiWheelC
                 if (srcId != -1)
                 {
                     Leave(srcId);
-                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
+                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
                 }
             }
         }
-        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        
-        {
-            while (!TryLock(dstId))
-            {
-                Thread.Sleep(50);
-            }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            new DriveTask(new LineTracking()
-            {
-                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-                //LeaveSrcFunction = Leave,
-                //SrcId = srcId,
-            }.Get()).Wait();
-
-            new DriveTask(new DstTracker()
-            {
-                Src = new Vector2(srcX, srcY),
-                Dst = new Vector2(dstX, dstY),
-            }.Get());Wait();
-            Leave(srcId);
-            DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
-
-        }
 
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
         {

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -390,13 +390,6 @@ namespace MultiWheelC
                     if (!running) break;
                     yield return true;
                 }
-                // 衔接速度：避免第一段结束后瞬间“掉速”，让第二段接管更连贯
-                var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-                var handoverSpeed = Math.Max(0.08f, Math.Min(PilotDefinition.Conf.DstTrackerMaxSpeed, 0.2f));
-                var direction = Math.Sign(dst.x - src.x);
-                if (direction == 0) direction = 1;
-                chassis.SendXYThSpeed(handoverSpeed * direction, 0f, 0f);
-                yield return true;
                 foreach (var running in new DstTracker()
                 {
                     Src = src,
@@ -415,7 +408,87 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
+    [MovementTest(name = "测试先离车再终点跟踪")]
+    public class LeaveCarThenDstTrackerTest : MovementTest
+    {
+        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
 
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                var following = new TireFollowing()
+                {
+                    GetController = () => new ChassisController().Get(),
+                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                    detectors = new List<TireFollowing.DetectorDefinition>()
+                    {
+                        new TireFollowing.DetectorDefinition()
+                        {
+                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                            StartGuessingY = 0,
+                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                            PathTransformation = new Tuple<float, float, float>(
+                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                                0),
+                        },
+                    },
+                    CarDirection = 180f,
+                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                    WalkBlindTh = 0,
+                    TireNum = 1
+                };
+
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+
+    [MovementTest(name = "驱动器下使能测试")]
     public class DriverDisableTest : MovementTest
     {
         public override void TestStop()

MultiWheel/MultiWheelC/Movements.cs

@@ -25,6 +25,7 @@ using System.Numerics;
 using System.Reflection;
 using System.Text;
 using System.Threading;
+using static ClumsyCore.DTools.Painter;
 
 namespace MultiWheelC
 {
@@ -208,43 +209,20 @@ namespace MultiWheelC
         public Action<int> LeaveSrcFunction = null;
         private PIDController pid;
 
-        // GhostMode 虚拟里程计
-        private float _ghostDistance;
-        private DateTime _lastTick;
-
         public override IEnumerable<bool> Get()
         {
-            bool isGhost = PilotDefinition.Self.GhostMode;
-            if (isGhost)
-            {
-                _ghostDistance = 0f;
-                _lastTick = DateTime.Now;
-            }
-
             pid = new PIDController(() =>
-                isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
                 Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
             { SpeedAccPerSec = MaxSpeed / 2f };
 
             var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
             while (true)
             {
                 var speed = pid.GetResponse(Target);
-
+                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
-                if (isGhost)
+                
-                {
-                    var now = DateTime.Now;
-                    float dt = (float)(now - _lastTick).TotalSeconds;
-                    _ghostDistance += speed * dt * 1000f;
-                    _lastTick = now;
-                    Console.WriteLine($"[Ghost] output: {speed:F2}  current: {_ghostDistance:F2}");
-                }
-                else
-                {
-                    Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
-                }
-
-                var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
                 chassis.SendXYThSpeed(speed, 0, 0);
 
                 if (pid.IsArrived()) break;
@@ -261,26 +239,60 @@ namespace MultiWheelC
 
     public class DriverAble : MovementDefinition
     {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器上使能");
             PilotDefinition.Self.ResetFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.ResetFromC = false;
-            Console.WriteLine("驱动器上使能完成");
+            if (success)
+                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }
 
     public class DriverDisable : MovementDefinition
     {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器下使能");
             PilotDefinition.Self.DisableFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = !PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = !PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.DisableFromC = false;
-            Console.WriteLine("驱动器下使能完成");
+            if (success)
+                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -132,6 +132,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
+    [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
 
     private float _multiVehicleAccumulateTh;
     private DateTime _multiVehicleLastThTime = DateTime.Now;