From 8b19a15fbb8dcba8c93ba27cf1bd0d740d4342f0 Mon Sep 17 00:00:00 2001 From: "shuai.li" <1248399577@qq.com> Date: Mon, 29 Jun 2026 11:46:02 +0800 Subject: [PATCH] =?UTF-8?q?update=20=E5=A2=9E=E5=8A=A0=E5=81=9C=E8=BD=A6?= =?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E9=92=BB=E8=BD=A6=E7=AD=89=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=20=E5=AE=9E=E7=8E=B0=E5=81=9C=E8=BD=A6=E6=9C=BA?= =?UTF-8?q?=E5=99=A8=E4=BA=BA=E8=83=BD=E5=8A=9B=E7=9A=84=E7=98=A6=E8=BA=AB?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MultiWheel/MultiWheelC/AGV.cs | 281 ++++++++++- .../MovementTests.TireFollowing.cs | 368 ++++++++++++++ MultiWheel/MultiWheelC/Movements.cs | 245 +++++++++ MultiWheel/MultiWheelC/MultiWheelC.csproj | 2 + MultiWheel/MultiWheelC/PilotConfig.cs | 55 +++ MultiWheel/MultiWheelC/PilotDefinition.cs | 29 ++ MultiWheel/MultiWheelC/TireFollowing.cs | 467 ++++++++++++++++++ 7 files changed, 1443 insertions(+), 4 deletions(-) create mode 100644 MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs create mode 100644 MultiWheel/MultiWheelC/Movements.cs create mode 100644 MultiWheel/MultiWheelC/TireFollowing.cs diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 718e02d..106eda9 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/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() + { + 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( + 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( + 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() + { + 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( + 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(str1); + Console.WriteLine(setLocationRes.l_step); + if (setLocationRes != null && setLocationRes.l_step == 2) break; + } + }); + + } + } } diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs new file mode 100644 index 0000000..f3693ac --- /dev/null +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -0,0 +1,368 @@ +锘縰sing 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 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 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() + { + 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() + { + 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( + 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( + 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() + { + 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( + 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); + } + } + +} + + diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs new file mode 100644 index 0000000..06d84d5 --- /dev/null +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -0,0 +1,245 @@ +锘縰sing 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 + { + /// + /// 鏃嬭浆鐩爣瑙掑害 + /// + public float AngleTarget; + + public float MaxSpeed; + + public Func ThetaReader = () => (float)DetourInterface.getCartLocation().th; + + public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis; + + public Func PidparamsRead = () => new PIDParams() { }; + + public PIDController thPid; + + private static float RangeAngle(float theta) + { + return (float)(theta - Math.Round(theta / 360.0f) * 360); + } + + public override IEnumerable 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 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 Get() + { + var start = DateTime.Now; + while ((DateTime.Now-start).TotalSeconds LeaveSrcFunction = null; + public Painter painter = UI.GetPainter("Line", false); + public override IEnumerable 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 LeaveSrcFunction = null; + private PIDController pid; + + public override IEnumerable 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 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 Get() + { + Console.WriteLine("椹卞姩鍣ㄤ笅浣胯兘"); + PilotDefinition.Self.DisableFromC = true; + Thread.Sleep(100); + PilotDefinition.Self.DisableFromC = false; + Console.WriteLine("椹卞姩鍣ㄤ笅浣胯兘瀹屾垚"); + yield return false; + } + } +} diff --git a/MultiWheel/MultiWheelC/MultiWheelC.csproj b/MultiWheel/MultiWheelC/MultiWheelC.csproj index a279414..8962a0d 100644 --- a/MultiWheel/MultiWheelC/MultiWheelC.csproj +++ b/MultiWheel/MultiWheelC/MultiWheelC.csproj @@ -10,6 +10,8 @@ + + diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index b4916f5..cd27e9e 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/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 = "杞儙璺熻釜:鍓嶉浄杈捐瘑鍒矾寰勫亸绉籜")] public float TireFollowingFrontLidarPathTransformationX = 253f; + [FieldMember(desc = "杞儙璺熻釜:鍓嶉浄杈捐瘑鍒矾寰勫亸绉籝")] public float TireFollowingFrontLidarPathTransformationY = 13f; + [FieldMember(desc = "杞儙璺熻釜:鍓嶉浄杈捐瘑鍒矾寰勫亸绉籘h")] public float TireFollowingFrontLidarWalkBlindTh = -1f; + [FieldMember(desc = "杞儙璺熻釜:鍚庨浄杈捐瘑鍒矾寰勫亸绉籜")] public float TireFollowingBackLidarPathTransformationX = 148f; + [FieldMember(desc = "杞儙璺熻釜:鍚庨浄杈捐瘑鍒矾寰勫亸绉籝")] public float TireFollowingBackLidarPathTransformationY = 2f; + [FieldMember(desc = "杞儙璺熻釜:鍚庨浄杈捐瘑鍒矾寰勫亸绉籘h")] 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 } diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index f6cb79c..d45e1c7 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -55,6 +55,35 @@ public class PilotDefinition : MultiWheelPilotDefinition GetController; + + /// + /// 杞﹁締鏂瑰悜 + /// + public float CarDirection = 0; + + /// + /// 鍋滄璺濈 + /// + //public float FinishDistance = 1000; + + /// + /// 鍑忛熻窛绂 + /// + public float SlowDistance = 1000; + + /// + /// 鏈澶ч熷害 + /// + public float MaxSpeed = 0.3f; + + /// + /// 閽昏疆鑳庢暟閲 + /// + public int TireNum = 1; + + /// + /// 鐩茶蛋瑙掑害鍋忕Щ + /// + public float WalkBlindTh = -1f; + + /// + /// 鏄惁妫娴嬪埌鐩爣 + /// + public bool NoTarget = false; + + public float GuessRangeX; + + public float GuessRangeY; + + /// + /// 妫娴嬪櫒瀹氫箟 + /// + public class DetectorDefinition + { + /// + /// 寮濮嬫娴嬭窛绂 + /// + public float StartGuessingX; + + /// + /// 寮濮嬫娴嬭窛绂 + /// + public float StartGuessingY; + + /// + /// 妫娴嬪嚱鏁 + /// + public Func, LineSegment> DetectFunction = null; + + public Action LeaveSrcFunction = null; + + public int SrcId = -1; + public int DstId = -1; + + /// + /// 璺緞鍋忕Щ + /// + public Tuple PathTransformation = Tuple.Create(0f, 0f, 0f); + + public float PathTransformationAnchorDistance = 0f; + + /// + /// 鍒囨崲鏉′欢 + /// + public Func SwitchWalkBlindCondition = null; + + /// + /// 鐩茶蛋鍋滄璺濈 + /// + public Func FinishWalkBlindCondition = null; + } + + public Func FinishCondition; + + /// + /// 澶氫釜妫娴嬪櫒鍒楄〃 + /// + public List detectors = null; + + private Painter _painter; + private List _remainDistanceList = new List(); + private List _remainAngleList = new List(); + + private List 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() + { + 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(); + } + + /// + ///璁＄畻杞︿綋涓績鐨勪綅绉诲拰瑙掑害澧為噺 + /// + /// a杞湪杞︿綋鍧愭爣绯讳笅浣嶇疆 + /// a杞湪杞︿綋鍧愭爣绯讳笅浣嶇Щ澧為噺 + /// b杞湪杞︿綋鍧愭爣绯讳笅浣嶇疆 + /// b杞湪杞︿綋鍧愭爣绯讳笅浣嶇Щ澧為噺 + /// + 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 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 { lfDelta, lrDelta, rfDelta, rrDelta }; + + var xs = new List(); + var ys = new List(); + var ths = new List(); + 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 { 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 { 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; + } +}