update 增加停车机器人钻车等代码 实现停车机器人能力的瘦身代码

MultiWheel/MultiWheelC/AGV.cs

@@ -1,9 +1,282 @@
+using ClumsyCore;
+using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
 using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using OpenCvSharp.Dnn;
+using OpenCvSharp.XFeatures2D;
+using System;
+using System.Collections.Generic;
+using System.Net.Http;
+using System.Numerics;
+using System.Threading;
+using System.Threading.Tasks;
+using static ClumsyCore.DTools.Painter;
+using static OpenCvSharp.ConnectedComponents;
 
-namespace MultiWheelC;
-
-public class AGV : BasicInterface
+namespace MultiWheelC
 {
-    public override AbstractGeometricController GetController() => new ChassisController().Get();
+    public class SetLocationRes
+    {
+        public float x, y, th;
+        public int l_step;
+        public long tick;
+        public string error;
+    }
+    public class AGV : MultiWheelInterface
+    {
+        public override AbstractGeometricController GetController()
+        {
+            return new ChassisController().Get();
+        }
+
+        public override MultiWheelMagTracker GetMagController()
+        {
+            return new MultiWheelMagTracker();
+        }
+
+        public override NaiveMagnetController GetNaiveMagnetController()
+        {
+            return new NaiveMagnetController();
+        }
+
+        public void Sleep(float s)
+        {
+            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
+        }
+
+        public void ControlChargePort(bool open)
+        {
+            DLog.Log($"call ControlChargePort({open})");
+            PilotDefinition.Self.OpenChargeByClumsy = open;
+        }
+
+        public void SwitchLidarArea(int area)
+        {
+            DLog.Log($"call SwitchLidarArea({area})");
+            PilotDefinition.Self.AreaChoose = area;
+        }
+
+        public void SwitchIoArea(int area)
+        {
+            if (area != -1)
+            {
+                PilotDefinition.Self.IOObstacleArea = area;
+            }
+        }
+
+        //参数1：tireNum 需要钻过的轮胎对数量
+        //参数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");
+            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
+            if (tireNum != 1 && tireNum != 2)
+            {
+                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
+                return;
+            }
+
+            if (PilotDefinition.Self.GhostMode)
+            {
+                while (!TryLock(dstId))
+                {
+                    Console.WriteLine("等待锁取货点中...");
+                    Thread.Sleep(200);
+                }
+                Console.WriteLine($"锁点{dstId}完成");
+                Thread.Sleep(1000);
+                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
+                Thread.Sleep(1000);
+                Leave(srcId);
+                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
+                Thread.Sleep(2000);
+                //Leave(dstId);
+                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
+                Thread.Sleep(2000);
+                Console.WriteLine($"结束钻车动作");
+                return;
+            }
+
+            var detectors = new List<TireFollowing.DetectorDefinition>()
+            {
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0),
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    DstId = dstId,
+                },
+                new TireFollowing.DetectorDefinition()
+                {
+                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
+                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                    StartGuessingY = 0,
+                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                    PathTransformation = new Tuple<float, float, float>(
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                    0)
+                },
+            };
+            DLog.Log($"检测器数量为{detectors.Count}", "TireFollowing");
+            var following = new TireFollowing()
+            {
+                GetController = () => new ChassisController().Get(),
+                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                detectors = detectors,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = detectors.Count,
+                CarDirection = frontLidarDetect ? 0f : 180f,
+                WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            var _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log("钻车动作结束", "TireFollowing");
+        }
+
+        //离车一定是后雷达识别一个轮胎
+        public void LeaveCar(int srcId, int dstId)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
+
+            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),
+                        LeaveSrcFunction = Leave,
+                        SrcId = srcId,
+                        DstId = dstId,
+                    },
+                },
+                CarDirection = 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                WalkBlindTh = 0,
+                TireNum = 1
+            };
+            var _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log("钻车动作结束", "TireFollowing");
+        }
+
+        //驱动器上使能
+        public void DriverAble()
+        {
+            var dl = new DriveTask(new DriverAble() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器上使能完成", "TireFollowing");
+        }
+
+        //驱动器下使能
+        public void DriverDisable()
+        {
+            var dl = new DriveTask(new DriverDisable() { }.Get());
+            dl.Wait();
+            DLog.Log("驱动器下使能完成", "TireFollowing");
+        }
+
+        //夹抱 close为true时夹抱，否则为还原
+        public void ClamptoTarget(bool close)
+        {
+            if (PilotDefinition.Self.GhostMode)
+            {
+                Thread.Sleep(2000);
+                Console.WriteLine("夹抱完成");
+                return;
+            }
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
+            }.Get()).Wait();
+        }
+
+        public void LineTracking(int srcId, int dstId, float LineDistance)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            new DriveTask(new LineTracking()
+            {
+                Target = LineDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                LeaveSrcFunction = Leave,
+                SrcId = srcId,
+            }.Get()).Wait();
+        }
+
+        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
+        {
+            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
+            PilotDefinition.Self.SlowDistance = slowDistance;
+            PilotDefinition.Self.StopDistance = stopDistance;
+        }
+        public void ChangeAvoidanceParam(float length = -1, float width = -1)
+        {
+
+            PilotDefinition.Self.CarLength = length;
+            PilotDefinition.Self.CarWidth = width;
+
+        }
+        public void SetLocation(float x, float y, float th)
+        {
+            DLog.Log($"call SetLocation({x},{y},{th})");
+            Console.WriteLine($"call SetLocation({x},{y},{th})");
+            Queue(() =>
+            {
+                while (true)
+                {
+                    var str1 = new HttpClient()
+                        .GetStringAsync(
+                            $"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
+                        .Result;
+                    Thread.Sleep(500);
+                    Console.WriteLine($"SetLocation str={str1}");
+                    var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
+                    Console.WriteLine(setLocationRes.l_step);
+                    if (setLocationRes != null && setLocationRes.l_step == 2) break;
+                }
+            });
+
+        }
+    }
 }

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -0,0 +1,368 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyDance.Detectors;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using OpenCvSharp.Dnn;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Security.Cryptography;
+using System.Threading;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    [MovementTest(name = "轮胎检测")]
+    public class TireDetect : MovementTest
+    {
+        public override void TestStop()
+        {
+            _running = false;
+        }
+
+        public override void Test()
+        {
+            _painter = UI.GetPainter("TwoLegDetectTest", false);
+            _painter.Clear();
+
+            var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(frontlidar.ToString());
+            var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
+            var lastDetectY = 0f;
+            while (_running)
+            {
+                var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
+                if(ld == null)
+                {
+                    //Console.WriteLine("ld == null");
+                    continue;
+                }
+                _painter.Clear();
+                var center = (ld.Src + ld.Dst) / 2;
+                var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
+                var distanceLabelPos = center / 2;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
+                _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+                lastDetectX = center.X;
+                lastDetectY = center.Y;
+                Thread.Sleep(100);
+            }
+        }
+
+        public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
+        {
+            return new Lidar2dDetect2LegTray()
+            {
+                BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
+                BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
+                BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
+                CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
+                LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
+                LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
+                Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
+                PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
+                SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
+            }.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
+                guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
+        }
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        private Painter _painter;
+        private bool _running = true;
+    }
+
+    [MovementTest(name = "钻车测试")]
+    public class FollowTire : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
+            var result = int.Parse(front.ToString());
+            var lidarname = result == 1 ? "前雷达" : "后雷达";
+            DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
+
+            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, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                    new TireFollowing.DetectorDefinition()
+                    {
+                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
+                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                        StartGuessingY = 0,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                        PathTransformation = new Tuple<float, float, float>(
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
+                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                        0)
+                    },
+                },
+                CarDirection = result == 1 ? 0f : 180f,
+                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                TireNum = PilotDefinition.Conf.TireFollowingTireNum,
+                WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束钻车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "离车测试")]
+    public class LeaveCar : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+
+        public override void Test()
+        {
+            DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
+            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.TireFollowingLeaveCarWalkBlindSwitchingDistance,
+                        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
+            };
+            _dt = new DriveTask(following.Get());
+            _dt.Wait();
+            DLog.Log($"结束离车测试", "TireFollowing");
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹关闭")]
+    public class ClampTest1 : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
+                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
+            }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "抱夹打开")]
+    public class ClampTest2 : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
+            }.Get()).Wait();
+        }
+    }
+    
+    [MovementTest(name = "测试前进基于轮里程")]
+    public class LineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试后退基于轮里程")]
+    public class ReverseLineTrackingTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+        }
+        public override void Test()
+        {
+            _dt = new DriveTask(new LineTracking()
+            {
+                Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+            }.Get());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "驱动器下使能测试")]
+    public class DriverDisableTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverDisable(){ }.Get()).Wait();
+        }
+    }
+
+    [MovementTest(name = "驱动器复位测试")]
+    public class DriverAbleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            new DriveTask(new DriverAble(){ }.Get()).Wait();
+        }
+    }
+
+    public class utils
+    {
+        public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
+        {
+            var means = CalculateMean(data);
+            var stdDevs = CalculateStandardDeviation(data, means);
+
+            return data.Where(point =>
+                Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
+                Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
+                AngularDistance(point.th, means.th) <= threshold * stdDevs.th
+            ).ToList();
+        }
+
+        public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
+        {
+            float meanX = data.Average(point => point.x);
+            float meanY = data.Average(point => point.y);
+            float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
+            float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
+            float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
+
+            return (meanX, meanY, meanTh);
+        }
+
+        public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
+        {
+            float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
+            float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
+
+            // 计算角度的方差
+            float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
+
+            return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
+        }
+
+        public static float DegreeToRadian(float degree)
+        {
+            return (float)(degree * Math.PI / 180.0);
+        }
+
+        public static float RadianToDegree(float radian)
+        {
+            return (float)(radian * 180.0 / Math.PI);
+        }
+
+        public static float AngularDistance(float angle1, float angle2)
+        {
+            return CommonMath.ThDiff(angle1, angle2);
+        }
+    }
+
+}
+
+

MultiWheel/MultiWheelC/Movements.cs

@@ -0,0 +1,245 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Sensors;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using ClumsyDance.Sensors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.HighLevelSecurity;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons;
+using MDCSToolBox.Commons.Controllers;
+using Newtonsoft.Json;
+using OpenCvSharp.Dnn;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Net.Http;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+
+namespace MultiWheelC
+{
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// 旋转目标角度
+        /// </summary>
+        public float AngleTarget;
+
+        public float MaxSpeed;
+
+        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+
+        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+
+        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+
+        public PIDController thPid;
+
+        private static float RangeAngle(float theta)
+        {
+            return (float)(theta - Math.Round(theta / 360.0f) * 360);
+        }
+
+        public override IEnumerable<bool> Get()
+        {
+            var targetAngle = RangeAngle(AngleTarget);
+            var p = PidparamsRead();
+            thPid = new PIDController(ThetaReader, p.Kp);
+            thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
+
+            DateTime lastTime = DateTime.Now;
+            while (true)
+            {
+                var s = thPid.GetResponse(targetAngle, true);
+                Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                Chassis.SendXYThSpeed(0, 0, s);
+                lastTime = DateTime.Now;
+                if (thPid.IsArrived()) break;
+                yield return true;
+            }
+            Chassis.SendXYThSpeed(0, 0, 0);
+            Console.WriteLine($"final rotate to {targetAngle}");
+        }
+    }
+
+
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        private PIDController leftpid, rightpid;
+
+        public override IEnumerable<bool> Get()
+        {
+            leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd, 0,
+                    ClampDeadZone, MaxClampSpeed)
+            { SpeedAccPerSec = MaxClampSpeed / 2f };
+
+            rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd, 0,
+                    ClampDeadZone, MaxClampSpeed)
+            { SpeedAccPerSec = MaxClampSpeed / 2f };
+
+            while (true)
+            {
+                var leftspeed = leftpid.GetResponse(LeftClampTarget);
+                var rightspeed = rightpid.GetResponse(RightClampTarget);
+                Console.WriteLine($"left arm speed:{leftspeed}  right arm speed:{rightspeed}");
+                PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                PilotDefinition.Self.SpeedRightArm = rightspeed;
+                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
+                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
+                //if(Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos) > PilotDefinition.Conf.ClampOutOfSyncThr)
+                //{
+                //    PilotDefinition.Self.ClampOutOfSync = true;
+                //    Console.WriteLine($"左夹臂位置{PilotDefinition.Self.LeftArmActualPos} 右夹臂位置{PilotDefinition.Self.RightArmActualPos}");
+                //    Console.WriteLine($"两个夹臂的位置差了{Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos)}");
+                //    break;
+                //}
+
+                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
+                yield return true;
+            }
+
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+            Console.WriteLine($"left clamp to target:{LeftClampTarget}  right clamp to target:{RightClampTarget}");
+        }
+    }
+
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2;
+        public override IEnumerable<bool> Get()
+        {
+            var start = DateTime.Now;
+            while ((DateTime.Now-start).TotalSeconds<Second)
+            {
+                yield return true;
+                Thread.Sleep(1000);
+                Console.WriteLine("Sleep");
+            }
+
+            yield return false;
+        }
+    }
+
+    //直线行走基于detour
+    public class LineTracking1 : MovementDefinition
+    {
+        public float LineDistance = 1000f;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        public Painter painter = UI.GetPainter("Line", false);
+        public override IEnumerable<bool> Get()
+        {
+            var curpose = DetourInterface.getCartLocation();
+            Console.WriteLine($"curpose.th:{curpose.th}");
+            var src = new Vector2((float)curpose.x, (float)curpose.y);
+            var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th), 
+                                  (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+            Console.WriteLine($"src:{src.X} {src.Y}");
+            Console.WriteLine($"dst:{dst.X} {dst.Y}");
+            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+            tracker.AddTrack(linePath);
+            var _dt = new DriveTask(tracker.Track());
+            _dt.Wait();
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    //直线行走基于轮里程
+    public class LineTracking : MovementDefinition
+    {
+        public float Target;
+        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+        public float Kp = PilotDefinition.Conf.LineTrackKp;
+        public float Ki = PilotDefinition.Conf.LineTrackKi;
+        public float Kd = PilotDefinition.Conf.LineTrackKd;
+        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        private PIDController pid;
+
+        public override IEnumerable<bool> Get()
+        {
+            pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
+                    DeadZone, MaxSpeed)
+            { SpeedAccPerSec = MaxSpeed / 2f };
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            while (true)
+            {
+                var speed = pid.GetResponse(Target);
+                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
+                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                chassis.SendXYThSpeed(speed, 0, 0);
+                //if (Math.Abs(current - Target) < pid.DeadZone)
+                //{
+                //    chassis.SendXYThSpeed(0f, 0f, 0f);
+                //    Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
+                //    break;
+                //}
+                if (pid.IsArrived()) break;
+                yield return true;
+            }
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
+            }
+            yield return false;
+        }
+    }
+
+    public class DriverAble : MovementDefinition
+    {
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器上使能");
+            PilotDefinition.Self.ResetFromC = true;
+            Thread.Sleep(100);
+            PilotDefinition.Self.ResetFromC = false;
+            Console.WriteLine("驱动器上使能完成");
+            yield return false;
+        }
+    }
+
+    public class DriverDisable : MovementDefinition
+    {
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine("驱动器下使能");
+            PilotDefinition.Self.DisableFromC = true;
+            Thread.Sleep(100);
+            PilotDefinition.Self.DisableFromC = false;
+            Console.WriteLine("驱动器下使能完成");
+            yield return false;
+        }
+    }
+}

MultiWheel/MultiWheelC/MultiWheelC.csproj

@@ -10,6 +10,8 @@
 
   <ItemGroup>
     <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
+    <PackageReference Include="OpenCvSharp4" Version="4.10.0.20240615" />
+    <PackageReference Include="OpenCvSharp4.Extensions" Version="4.10.0.20240615" />
     <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
   </ItemGroup>
 

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -131,4 +131,59 @@ public class PilotConfig : MultiWheelPilotConfig
 
     [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
     public float TwoLegFilterWidth = 600f;
+
+    #region
+    [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
+    [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
+    [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
+    [FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
+    [FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;
+
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
+    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
+    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
+
+    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
+    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
+    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
+    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
+    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
+    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
+    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
+    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
+
+    [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
+    [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
+    [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
+    [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
+    [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
+    [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
+
+    [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
+    [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
+    [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
+    [FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
+    [FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
+    [FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
+    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
+    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
+    [FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
+    [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
+
+    [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
+    [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
+    #endregion
 }

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -55,6 +55,35 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
     [AsLowerIO(desc = "车号")] public int CarNum = 1;
 
+    #region 夹臂变量
+    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
+
+    [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
+
+    [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
+
+    [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
+
+    [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
+    #endregion
+
+    [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
+    [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
+    [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
+    [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
+    [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
+    [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
+    [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
+    [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
+
+    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
+
+    [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
+    [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
+
     private float _multiVehicleAccumulateTh;
     private DateTime _multiVehicleLastThTime = DateTime.Now;
     private readonly object _multiVehicleNotificationLock = new();

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -0,0 +1,467 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Reflection;
+using System.Text;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using ClumsyCore.Utilities;
+using ClumsyDance.ClumsyWalk.Detectors;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox;
+using MDCSToolBox.Clumsy.Calibration;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using static ClumsyCore.DTools.Painter;
+using LineSegment = ClumsyCore.Utilities.LineSegment;
+
+namespace MultiWheelC
+{
+    public class TireFollowing : MovementDefinition
+    {
+        public Func<AbstractGeometricController> GetController;
+
+        /// <summary>
+        /// 车辆方向
+        /// </summary>
+        public float CarDirection = 0;
+
+        /// <summary>
+        /// 停止距离
+        /// </summary>
+        //public float FinishDistance = 1000;
+
+        /// <summary>
+        /// 减速距离
+        /// </summary>
+        public float SlowDistance = 1000;
+
+        /// <summary>
+        /// 最大速度
+        /// </summary>
+        public float MaxSpeed = 0.3f;
+
+        /// <summary>
+        /// 钻轮胎数量
+        /// </summary>
+        public int TireNum = 1;
+
+        /// <summary>
+        /// 盲走角度偏移
+        /// </summary>
+        public float WalkBlindTh = -1f;
+
+        /// <summary>
+        /// 是否检测到目标
+        /// </summary>
+        public bool NoTarget = false;
+
+        public float GuessRangeX;
+
+        public float GuessRangeY;
+
+        /// <summary>
+        /// 检测器定义
+        /// </summary>
+        public class DetectorDefinition
+        {
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingX;
+
+            /// <summary>
+            /// 开始检测距离
+            /// </summary>
+            public float StartGuessingY;
+
+            /// <summary>
+            /// 检测函数
+            /// </summary>
+            public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
+
+            public Action<int> LeaveSrcFunction = null;
+
+            public int SrcId = -1;
+            public int DstId = -1;
+
+            /// <summary>
+            /// 路径偏移
+            /// </summary>
+            public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
+
+            public float PathTransformationAnchorDistance = 0f;
+
+            /// <summary>
+            /// 切换条件
+            /// </summary>
+            public Func<float, bool> SwitchWalkBlindCondition = null;
+
+            /// <summary>
+            /// 盲走停止距离
+            /// </summary>
+            public Func<float, bool> FinishWalkBlindCondition = null;
+        }
+
+        public Func<bool> FinishCondition;
+
+        /// <summary>
+        /// 多个检测器列表
+        /// </summary>
+        public List<DetectorDefinition> detectors = null;
+
+        private Painter _painter;
+        private List<float> _remainDistanceList = new List<float>();
+        private List<float> _remainAngleList = new List<float>();
+
+        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
+        {
+            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
+            painter.Clear();
+            painter.Clear(3000);
+
+            var box = new Vector2[]
+            {
+                new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
+                new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
+                new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
+            };
+
+            for (var i = 0; i < box.Length; ++i)
+                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
+
+            // PC filter in car coordinate frame
+            return new List<DetectFilter>()
+            {
+                new(CoordinateSystem.Car2D,
+                    p => LessMath.IsPointInPolygon4(
+                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
+            };
+        }
+
+        public void Stop()
+        {
+            _dt?.Stop();
+        }
+
+        /// <summary>
+        ///计算车体中心的位移和角度增量
+        /// </summary>
+        /// <param name="a">a轮在车体坐标系下位置</param>
+        /// <param name="va">a轮在车体坐标系下位移增量</param>
+        /// <param name="b">b轮在车体坐标系下位置</param>
+        /// <param name="vb">b轮在车体坐标系下位移增量</param>
+        /// <returns></returns>
+        private static (float, float, float) CenterMoveFromPoints(Vector2 a,
+            Vector2 aDelta,
+            Vector2 b,
+            Vector2 bDelta)
+        {
+            float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
+            var eps = 0.0000001;
+            if (Math.Abs(a.Y - b.Y) > eps)
+            {
+                th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
+            }
+            if (Math.Abs(a.X - b.X) > eps)
+            {
+                th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
+            }
+            th = th_x == 0 ? th_y : th_x;
+
+            x = (aDelta.X + bDelta.X) / 2 - (a.Y - b.Y) / 2 * th;
+
+            y = (aDelta.Y + bDelta.Y) / 2 + (a.X - b.X) / 2 * th;
+
+            return (x, y, th);
+        }
+
+
+        public override IEnumerable<bool> Get()
+        {
+            _painter = UI.GetPainter("GeneralFollowing", false);
+            var lastDetectX = detectors[0].StartGuessingX;
+            var lastDetectY = detectors[0].StartGuessingY;
+            var detectorIndex = 0;
+
+            var controller = (MultiWheelGeometricController)GetController.Invoke();
+            controller.BaseSpeed = MaxSpeed;
+            controller.FinishDistance = float.MinValue;
+            controller.FirstThAccuracy = 999;
+            _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
+
+            float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
+            bool WalkBlindStage1 = false, WalkBlindStage2 = false;
+            var angle2target = -1f;
+            float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
+            float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
+
+            (float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
+            {
+                // Read current encoders
+                var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                var curLFREncoder = PilotDefinition.Self.LFRActualPos;
+                var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                var curRFREncoder = PilotDefinition.Self.RFRActualPos;
+                var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                var curLRREncoder = PilotDefinition.Self.LRRActualPos;
+                var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                var curRRREncoder = PilotDefinition.Self.RRRActualPos;
+
+                // Average delta per wheel pair (LF, LR, RF, RR)
+                var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2;
+                var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2;
+                var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2;
+                var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2;
+                var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
+
+                var xs = new List<float>();
+                var ys = new List<float>();
+                var ths = new List<float>();
+                var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
+                var steerWheels = chassis.GetSteerWheels();
+                for (var i = 0; i < steerWheels.Count; ++i)
+                {
+                    var sw1 = steerWheels[i];
+                    var a = sw1.Position;
+                    var tha = sw1.ReadAngle() / 180 * (float)Math.PI;
+                    var deltaa = deltaList[i];
+                    var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
+                    for (var j = i + 1; j < steerWheels.Count; ++j)
+                    {
+                        var sw2 = steerWheels[j];
+                        var b = sw2.Position;
+                        var thb = sw2.ReadAngle() / 180 * (float)Math.PI;
+                        var deltab = deltaList[j];
+                        var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
+                        var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
+                        Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180:f2} ", $"{i}_{j}");
+                        xs.Add(tempx);
+                        ys.Add(tempy);
+                        ths.Add(tempth);
+                    }
+                }
+                var x = xs.Average();
+                var y = ys.Average();
+                var Th = ths.Average() / (float)Math.PI * 180;
+                var moveTup = Tuple.Create(x, y, Th);
+                var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
+
+                _lastLFLEncoder = curLFLEncoder;
+                _lastLFREncoder = curLFREncoder;
+                _lastRFLEncoder = curRFLEncoder;
+                _lastRFREncoder = curRFREncoder;
+                _lastLRLEncoder = curLRLEncoder;
+                _lastLRREncoder = curLRREncoder;
+                _lastRRLEncoder = curRRLEncoder;
+                _lastRRREncoder = curRRREncoder;
+                return (moved.Item1, moved.Item2, moved.Item3);
+            }
+
+            while (true)
+            {
+                if (detectorIndex > detectors.Count - 1)
+                    throw new Exception("detector index out of range!");
+
+                _painter.Clear();
+                if (WalkBlindStage1 || WalkBlindStage2)
+                {
+                    //第二次盲走时或只钻一个轮胎时
+                    if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
+                    {
+                        controller.SlowDistance = SlowDistance;
+                        controller.SlowingPow = 0.7f;
+                    }
+                    (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
+                    var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
+                    var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
+                    var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
+                    var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
+
+                    DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
+                    _painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
+                    _painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
+
+                    var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
+                        walkBlindPath.Src.X, walkBlindPath.Src.Y);
+                    _remainDistanceList.Add(rd);
+                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
+                    rd = _remainDistanceList.Average();
+                    DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
+                    // 检查是否达到盲走结束条件
+                    if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
+                    {
+                        if(WalkBlindStage1)
+                        {
+                            DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
+                            //if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //{
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
+                            //    DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
+                            //}
+                            WalkBlindStage1 = false;
+                            _remainAngleList.Clear();
+                            _remainDistanceList.Clear();
+                            detectorIndex++;
+                            if (detectors.Count == 1 || TireNum == 1)
+                            {
+                                _dt.Stop();
+                                yield return false;
+                            }
+                        }
+                        else if(WalkBlindStage2)
+                        {
+                            DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
+                            _dt.Stop();
+                            yield return false;
+                        }
+                    }
+                    yield return true;
+                    continue;
+                }
+
+                var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
+                        SetFilters(lastDetectX, lastDetectY));
+
+                if(target == null)
+                {
+                    DLog.Log("无目标，等待下一帧", "TireFollowing");
+                    controller.FirstRotateMaxSpeed = 0;
+                    yield return true;
+                    continue;
+                }
+                else controller.FirstRotateMaxSpeed = 5;
+
+                var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
+                var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2, (target.Src.Y + target.Dst.Y) / 2);
+                var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
+                //距离较近以后角度容易跳变
+                if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
+                {
+                    yield return true;
+                    continue;
+                }
+                else _remainAngleList.Add(targetAngle);
+                while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
+                angle2target = _remainAngleList.Average();
+                var distanceLabelPos = targetPos / 2;
+                _painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
+                _painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
+
+                var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen:3000, 
+                                                          bias: detectors[detectorIndex].PathTransformation,
+                                                          biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
+
+                if (path == null)
+                {
+                    DLog.Log("no path!", "TireFollowing");
+                    NoTarget = true;
+                }
+                else
+                {
+                    lastDetectX = ((target.Src + target.Dst) / 2).X;
+                    lastDetectY = ((target.Src + target.Dst) / 2).Y;
+                    _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true);
+
+                    var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y,
+                        path.CarPath.Src.X, path.CarPath.Src.Y);
+                    _remainDistanceList.Add(rd);
+                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
+                    rd = _remainDistanceList.Average();
+                    _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
+
+                    if (detectorIndex < detectors.Count - 1)
+                    {
+                        controller.SlowDistance = 1;
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage1 = true;
+                            if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            {
+                                detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                                DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            }
+                            WalkBlindCarPathDstX = path.CarPath.Dst.X;
+                            WalkBlindCarPathDstY = path.CarPath.Dst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
+                            _remainDistanceList.Clear();
+                            lastDetectX = detectors[detectorIndex+1].StartGuessingX;
+                            lastDetectY = detectors[detectorIndex+1].StartGuessingY;
+                            continue;
+                        }
+                    }
+                    else if (detectorIndex == detectors.Count - 1)
+                    {
+                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
+                        {
+                            WalkBlindStage2 = true;
+                            WalkBlindCarPathDstX = path.CarPath.Dst.X;
+                            WalkBlindCarPathDstY = path.CarPath.Dst.Y;
+                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
+                            DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
+                                                                $"{WalkBlindCarPathDstY:F2} " +
+                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
+                            if(detectors.Count == 1)
+                            {
+                                if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                                {
+                                    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                                    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                                }
+                            }
+
+                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
+                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
+                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
+                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
+                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
+                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
+                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
+                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
+                            _remainDistanceList.Clear();
+                            continue;
+                        }
+                    }
+                    DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
+                    var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst);
+                    track.CarDirectionBias = CarDirection;
+                    controller.UpdateTracks(new List<AbstractTrack> { track });
+                    NoTarget = false;
+                }
+                yield return true;
+            }
+        }
+
+        private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
+        {
+            return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
+        }
+
+        private DriveTask _dt;
+    }
+}