Tutorial/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

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 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()
        {
            _dt?.Stop();
            PilotDefinition.Self.SpeedLeftArm = 0;
            PilotDefinition.Self.SpeedRightArm = 0;
        }

        public override void Test()
        {
            _dt = new DriveTask(new ClampToTarget()
            {
                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
            }.Get());
            _dt.Wait();
        }

        private DriveTask _dt;
    }

    [MovementTest(name = "抱夹打开")]
    public class ClampTest2 : MovementTest
    {
        public override void TestStop()
        {
            _dt?.Stop();
            PilotDefinition.Self.SpeedLeftArm = 0;
            PilotDefinition.Self.SpeedRightArm = 0;
        }

        public override void Test()
        {
            _dt = new DriveTask(new ClampToTarget()
            {
                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
            }.Get());
            _dt.Wait();
        }

        private DriveTask _dt;
    }

    [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 DstTrackerForward : MovementTest
    {
        public bool UseInteractivePick = true;
        public float srcX;
        public float srcY;
        public float dstX;
        public float dstY;
        public float carDirectionBias = 0f;
        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");

        public override void TestStop()
        {
            _dt?.Stop();
            _painter?.Clear();
        }
        public override void Test()
        {
            var p1 = UI.GetPoint("point1");
            var p2 = UI.GetPoint("point2");

            _painter.Clear();
            _dt = new DriveTask(new DstTracker()
            {
                Src = p1,
                Dst = p2,
                CarDirectionBias = carDirectionBias,
            }.Get());
            _dt.Wait();
        }

        private DriveTask _dt;
    }

    [MovementTest(name = "测试终点跟踪动作-后退")]
    public class DstTrackerhoutui : MovementTest
    {
        public bool UseInteractivePick = true;
        public float srcX;
        public float srcY;
        public float dstX;
        public float dstY;
        public float carDirectionBias = 180f;
        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");

        public override void TestStop()
        {
            _dt?.Stop();
            _painter?.Clear();
        }
        public override void Test()
        {
            var p1 = UI.GetPoint("point1");
            var p2 = UI.GetPoint("point2");

            _painter.Clear();
            _dt = new DriveTask(new DstTracker()
            {
                Src = p1,
                Dst = p2,
                CarDirectionBias = carDirectionBias,
            }.Get());
            _dt.Wait();
        }

        private DriveTask _dt;
    }

    [MovementTest(name = "测试先直行再终点跟踪")]
    public class LineTrackThenDstTrackerTest : MovementTest
    {
        public float carDirectionBias = 0f;
        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
        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> TrackThenFollow()
            {
                foreach (var running in new LineTracking()
                {
                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
                    EnableHandover = true,
                    HandoverDistance = 200f,
                    HandoverSpeed = 0.3f,
                }.Get())
                {
                    if (!running) break;
                    yield return true;
                }
                foreach (var running in new DstTracker()
                {
                    Src = src,
                    Dst = dst,
                    CarDirectionBias = carDirectionBias,
                    InitialSendSpeed = 0.3f
                }.Get())
                {
                    if (!running) break;
                    yield return true;
                }
                yield return false;
            }
            _dt = new DriveTask(TrackThenFollow());
            _dt.Wait();
        }
        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()
        {
            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);
        }
    }

}