From 22670d916bcb9a53752ae98b24c014b9f35d0718 Mon Sep 17 00:00:00 2001 From: "shuai.li" <1248399577@qq.com> Date: Sat, 4 Jul 2026 14:58:03 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0dstTracker?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MultiWheel/MultiWheelC/AGV.cs | 155 +++++++++++------- .../MovementTests.TireFollowing.cs | 87 +++++++++- MultiWheel/MultiWheelC/Movements.cs | 74 +++++---- MultiWheel/MultiWheelC/PilotDefinition.cs | 1 + 4 files changed, 221 insertions(+), 96 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 7763deb..603fea8 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -1,5 +1,7 @@ using ClumsyCore; using ClumsyCore.Interfaces; +using ClumsyCore.Sensors; +using CommonUsage.Chassis; using FundamentalLib; using MDCSToolBox.Clumsy.AgvInterfaces; using MDCSToolBox.Clumsy.MotionControllers; @@ -10,6 +12,7 @@ using System; using System.Collections.Generic; using System.Net.Http; using System.Numerics; +using System.Security.Cryptography; using System.Threading; using System.Threading.Tasks; using static ClumsyCore.DTools.Painter; @@ -65,20 +68,20 @@ namespace MultiWheelC } } - //参数1：tireNum 需要钻过的轮胎对数量 - //参数2：frontLidarDetect true:前雷达识别 false:后雷达识别 + //鍙傛暟1锛歵ireNum 闇瑕侀捇杩囩殑杞儙瀵规暟閲 + //鍙傛暟2锛歠rontLidarDetect 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"); + DLog.Log($"閿佺偣{dstId}瀹屾垚", "TireFollowing"); + var lidarName = frontLidarDetect ? "鍓嶉浄杈" : "鍚庨浄杈"; + DLog.Log($"寮濮嬮捇杞﹀姩浣滐紝閫氳繃{lidarName}璇嗗埆缁撴灉閽粄tireNum}瀵硅疆鑳", "TireFollowing"); if (tireNum != 1 && tireNum != 2) { - DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing"); + DLog.Log($"TireNum蹇呴』鏄1鎴2 (褰撳墠杈撳叆:{tireNum})", "TireFollowing"); return; } @@ -86,20 +89,20 @@ namespace MultiWheelC { while (!TryLock(dstId)) { - Console.WriteLine("等待锁取货点中..."); + Console.WriteLine("绛夊緟閿佸彇璐х偣涓..."); Thread.Sleep(200); } - Console.WriteLine($"锁点{dstId}完成"); + Console.WriteLine($"閿佺偣{dstId}瀹屾垚"); Thread.Sleep(1000); - Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎"); + Console.WriteLine($"寮濮嬮捇杞﹀姩浣滐紝閫氳繃{lidarName}璇嗗埆缁撴灉閽粄tireNum}瀵硅疆鑳"); Thread.Sleep(1000); Leave(srcId); - Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}"); + Console.WriteLine($"寮濮嬬涓娈电洸璧帮紝姝ゆ椂閲婃斁棰勫彇璐х偣{srcId}"); Thread.Sleep(2000); //Leave(dstId); - //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}"); + //Console.WriteLine($"缁撴潫绗竴娈电洸璧帮紝姝ゆ椂閲婃斁鍙栬揣鐐箋dstId}"); Thread.Sleep(2000); - Console.WriteLine($"结束钻车动作"); + Console.WriteLine($"缁撴潫閽昏溅鍔ㄤ綔"); return; } @@ -133,7 +136,7 @@ namespace MultiWheelC 0) }, }; - DLog.Log($"钻胎为{tireNum}", "TireFollowing"); + DLog.Log($"閽昏儙涓簕tireNum}", "TireFollowing"); var following = new TireFollowing() { GetController = () => new ChassisController().Get(), @@ -148,18 +151,18 @@ namespace MultiWheelC }; var _dt = new DriveTask(following.Get()); _dt.Wait(); - DLog.Log("钻车动作结束", "TireFollowing"); + DLog.Log("閽昏溅鍔ㄤ綔缁撴潫", "TireFollowing"); } - //离车一定是后雷达识别一个轮胎 + //绂昏溅涓瀹氭槸鍚庨浄杈捐瘑鍒竴涓疆鑳 public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) { while (!TryLock(dstId)) { Thread.Sleep(50); } - DLog.Log($"锁点{dstId}完成", "TireFollowing"); - DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing"); + DLog.Log($"閿佺偣{dstId}瀹屾垚", "TireFollowing"); + DLog.Log($"寮濮嬮捇杞﹀姩浣滐紝閫氳繃鍚庨浄杈捐瘑鍒粨鏋滈捇1瀵硅疆鑳", "TireFollowing"); var following = new TireFollowing() { @@ -173,7 +176,7 @@ namespace MultiWheelC DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false), StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX, StartGuessingY = 0, - SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance, + SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance, FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance, PathTransformation = new Tuple( PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX, @@ -190,39 +193,97 @@ namespace MultiWheelC WalkBlindTh = 0, TireNum = 1 }; - var _dt = new DriveTask(following.Get()); - _dt.Wait(); - new DriveTask(new DstTracker() + + IEnumerable LeaveThenFollow() { - Src = new Vector2(srcX, srcY), - Dst = new Vector2(dstX, dstY), - }.Get()); Wait(); - DLog.Log("钻车动作结束", "TireFollowing"); + foreach (var running in following.Get()) + { + if (!running) break; + yield return true; + } + DLog.Log($"閲婃斁閿佺偣{srcId}瀹屾垚", "TireFollowing"); + DLog.Log("绂昏溅TireFollowing缁撴潫锛屽紑濮婦stTracker", "TireFollowing"); + + foreach (var running in new DstTracker() + { + Src = new Vector2(srcX, srcY), + Dst = new Vector2(dstX, dstY), + CarDirectionBias = 180f, + }.Get()) + { + if (!running) break; + yield return true; + } + + yield return false; + } + + var _dt = new DriveTask(LeaveThenFollow()); + _dt.Wait(); + DLog.Log("绂昏溅鍔ㄤ綔1缁撴潫", "TireFollowing"); } - //驱动器上使能 + public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) + { + while (!TryLock(dstId)) + { + Thread.Sleep(50); + } + DLog.Log($"閿佺偣{dstId}瀹屾垚", "TireFollowing"); + IEnumerable TrackThenFollow() + { + foreach (var running in new LineTracking() + { + Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, + LeaveSrcFunction = Leave, + SrcId = srcId, + }.Get()) + { + if (!running) break; + yield return true; + } + DLog.Log("绂昏溅LineTracking缁撴潫锛屽紑濮婦stTracker", "TireFollowing"); + foreach (var running in new DstTracker() + { + Src = new Vector2(srcX, srcY), + Dst = new Vector2(dstX, dstY), + }.Get()) + { + if (!running) break; + yield return true; + } + yield return false; + } + var _dt = new DriveTask(TrackThenFollow()); + _dt.Wait(); + //Leave(srcId); + DLog.Log($"閲婃斁閿佺偣{srcId}瀹屾垚", "TireFollowing"); + DLog.Log("绂昏溅鍔ㄤ綔2缁撴潫", "TireFollowing"); + } + + //椹卞姩鍣ㄤ笂浣胯兘 public void DriverAble() { var dl = new DriveTask(new DriverAble() { }.Get()); dl.Wait(); - DLog.Log("驱动器上使能完成", "TireFollowing"); + DLog.Log("椹卞姩鍣ㄤ笂浣胯兘瀹屾垚", "TireFollowing"); } - //驱动器下使能 + //椹卞姩鍣ㄤ笅浣胯兘 public void DriverDisable() { var dl = new DriveTask(new DriverDisable() { }.Get()); dl.Wait(); - DLog.Log("驱动器下使能完成", "TireFollowing"); + DLog.Log("椹卞姩鍣ㄤ笅浣胯兘瀹屾垚", "TireFollowing"); } - // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。 + // 澶规姳锛歝lose 涓 true 鏃跺叧闂す鎶憋紝鍚﹀垯鎵撳紑澶规姳銆 public void ClamptoTarget(bool close) { if (PilotDefinition.Self.GhostMode) { Thread.Sleep(2000); - Console.WriteLine("夹抱完成"); + Console.WriteLine("澶规姳瀹屾垚"); return; } new DriveTask(new ClampToTarget() @@ -231,8 +292,8 @@ namespace MultiWheelC RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos }.Get()).Wait(); } - // 车队联动-自动蟹行：供调度按绝对起终点触发。 - // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。 + // 杞﹂槦鑱斿姩-鑷姩锜硅锛氫緵璋冨害鎸夌粷瀵硅捣缁堢偣瑙﹀彂銆 + // CarDirectionBias 琛ㄧず杞﹂槦鏂瑰悜鐩稿璺緞鏂瑰悜鐨勫す瑙掞紝閫嗘椂閽堜负姝ｃ public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId, float speed, float CarDirectionBias) { @@ -256,7 +317,7 @@ namespace MultiWheelC { Thread.Sleep(50); } - DLog.Log($"锁点{dstId}完成", "FleetCrabDbg"); + DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCrabDbg"); } var action = new MultiWheelC.FleetCrabWalk @@ -286,33 +347,11 @@ namespace MultiWheelC if (srcId != -1) { Leave(srcId); - DLog.Log($"释放放车点{srcId}", "FleetCrabDbg"); + DLog.Log($"閲婃斁鏀捐溅鐐箋srcId}", "FleetCrabDbg"); } } } - public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY) - { - while (!TryLock(dstId)) - { - Thread.Sleep(50); - } - DLog.Log($"锁点{dstId}完成", "TireFollowing"); - new DriveTask(new LineTracking() - { - Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, - //LeaveSrcFunction = Leave, - //SrcId = srcId, - }.Get()).Wait(); - - new DriveTask(new DstTracker() - { - Src = new Vector2(srcX, srcY), - Dst = new Vector2(dstX, dstY), - }.Get());Wait(); - Leave(srcId); - DLog.Log($"释放锁点{srcId}完成", "TireFollowing"); - - } + public void ChangeAvoidanceDistance(float stopDistance, float slowDistance) { diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index a3fc5fb..6f6c55a 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -390,13 +390,6 @@ namespace MultiWheelC if (!running) break; yield return true; } - // 琛旀帴閫熷害锛氶伩鍏嶇涓娈电粨鏉熷悗鐬棿鈥滄帀閫熲濓紝璁╃浜屾鎺ョ鏇磋繛璐 - var chassis = (MultiWheelChassis)PilotDefinition.Chassis; - var handoverSpeed = Math.Max(0.08f, Math.Min(PilotDefinition.Conf.DstTrackerMaxSpeed, 0.2f)); - var direction = Math.Sign(dst.x - src.x); - if (direction == 0) direction = 1; - chassis.SendXYThSpeed(handoverSpeed * direction, 0f, 0f); - yield return true; foreach (var running in new DstTracker() { Src = src, @@ -415,7 +408,87 @@ namespace MultiWheelC 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 LeaveThenFollow() + { + 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), + }, + }, + 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() diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 8480ae7..043a679 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -25,6 +25,7 @@ using System.Numerics; using System.Reflection; using System.Text; using System.Threading; +using static ClumsyCore.DTools.Painter; namespace MultiWheelC { @@ -208,43 +209,20 @@ namespace MultiWheelC public Action LeaveSrcFunction = null; private PIDController pid; - // GhostMode 铏氭嫙閲岀▼璁 - private float _ghostDistance; - private DateTime _lastTick; - public override IEnumerable Get() { - bool isGhost = PilotDefinition.Self.GhostMode; - if (isGhost) - { - _ghostDistance = 0f; - _lastTick = DateTime.Now; - } - pid = new PIDController(() => - isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0, DeadZone, MaxSpeed) { SpeedAccPerSec = MaxSpeed / 2f }; var chassis = (MultiWheelChassis)PilotDefinition.Chassis; + DLog.Log($"鐩寸嚎琛岄┒璺濈:{Target}", "TireFollowing"); while (true) { var speed = pid.GetResponse(Target); - - if (isGhost) - { - var now = DateTime.Now; - float dt = (float)(now - _lastTick).TotalSeconds; - _ghostDistance += speed * dt * 1000f; - _lastTick = now; - Console.WriteLine($"[Ghost] output: {speed:F2} current: {_ghostDistance:F2}"); - } - else - { - Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}"); - } - - var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2; + Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}"); + chassis.SendXYThSpeed(speed, 0, 0); if (pid.IsArrived()) break; @@ -261,26 +239,60 @@ namespace MultiWheelC public class DriverAble : MovementDefinition { + public int WaitTimeoutMs = 2000; + public int PollIntervalMs = 50; + public override IEnumerable Get() { Console.WriteLine("椹卞姩鍣ㄤ笂浣胯兘"); PilotDefinition.Self.ResetFromC = true; - Thread.Sleep(200); + + var start = DateTime.Now; + var timeoutMs = Math.Max(0, WaitTimeoutMs); + var pollMs = Math.Max(1, PollIntervalMs); + var success = PilotDefinition.Self.WheelAbleState; + while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs) + { + Thread.Sleep(pollMs); + success = PilotDefinition.Self.WheelAbleState; + if (!success) yield return true; + } + PilotDefinition.Self.ResetFromC = false; - Console.WriteLine("椹卞姩鍣ㄤ笂浣胯兘瀹屾垚"); + if (success) + Console.WriteLine($"椹卞姩鍣ㄤ笂浣胯兘瀹屾垚锛學heelAbleState={PilotDefinition.Self.WheelAbleState}"); + else + Console.WriteLine($"椹卞姩鍣ㄤ笂浣胯兘瓒呮椂锛學heelAbleState={PilotDefinition.Self.WheelAbleState}锛岀瓑寰厈timeoutMs}ms"); yield return false; } } public class DriverDisable : MovementDefinition { + public int WaitTimeoutMs = 3000; + public int PollIntervalMs = 20; + public override IEnumerable Get() { Console.WriteLine("椹卞姩鍣ㄤ笅浣胯兘"); PilotDefinition.Self.DisableFromC = true; - Thread.Sleep(200); + + var start = DateTime.Now; + var timeoutMs = Math.Max(0, WaitTimeoutMs); + var pollMs = Math.Max(1, PollIntervalMs); + var success = !PilotDefinition.Self.WheelAbleState; + while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs) + { + Thread.Sleep(pollMs); + success = !PilotDefinition.Self.WheelAbleState; + if (!success) yield return true; + } + PilotDefinition.Self.DisableFromC = false; - Console.WriteLine("椹卞姩鍣ㄤ笅浣胯兘瀹屾垚"); + if (success) + Console.WriteLine($"椹卞姩鍣ㄤ笅浣胯兘瀹屾垚锛學heelAbleState={PilotDefinition.Self.WheelAbleState}"); + else + Console.WriteLine($"椹卞姩鍣ㄤ笅浣胯兘瓒呮椂锛學heelAbleState={PilotDefinition.Self.WheelAbleState}锛岀瓑寰厈timeoutMs}ms"); yield return false; } } diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index bc7a704..e8b904d 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -132,6 +132,7 @@ public class PilotDefinition : MultiWheelPilotDefinition