Tutorial/MultiWheel/MultiWheelC/AGV.cs

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 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;
                }
            });

        }
    }
}