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

2026-06-29 11:46:02 +08:00
parent d935e847ec
commit 8b19a15fbb
7 changed files with 1443 additions and 4 deletions
@@ -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);
+        }
+    }
+
+}
+
+