diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 106eda9..5413d17 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -5,8 +5,6 @@ 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; @@ -14,7 +12,6 @@ using System.Numerics; using System.Threading; using System.Threading.Tasks; using static ClumsyCore.DTools.Painter; -using static OpenCvSharp.ConnectedComponents; namespace MultiWheelC { @@ -213,22 +210,6 @@ namespace MultiWheelC 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)) diff --git a/MultiWheel/MultiWheelC/ChassisController.cs b/MultiWheel/MultiWheelC/ChassisController.cs index 101f2b0..c041b2a 100644 --- a/MultiWheel/MultiWheelC/ChassisController.cs +++ b/MultiWheel/MultiWheelC/ChassisController.cs @@ -3,6 +3,7 @@ using System.Numerics; using ClumsyCore; using ClumsyCore.Interfaces; using ClumsyCore.Pilot; +using FundamentalLib; using MDCSToolBox.Clumsy.MotionControllers; using MDCSToolBox.Clumsy.Movements; using MDCSToolBox.Clumsy.Pilot; @@ -13,6 +14,8 @@ public class ChassisController : MovementDefinition { - PilotDefinition.Self.MultiVehicleAutoEnabled = true; - lock (PilotDefinition.Self.MultiVehicleFleet) + var self = PilotDefinition.Self; + self.MultiVehicleAutoEnabled = true; + // A: 鐢ㄥ浐瀹氶攣瀵硅薄锛堜笉鍐嶉攣浼氳鏇挎崲鐨勫瓧娈靛紩鐢級銆 + int fleetCnt; + lock (self.FleetLock) + fleetCnt = self.MultiVehicleFleet.Count; + + // 璇婃柇锛堣妭娴 ~300ms锛夛細纭鍥炶皟琚皟鐢ㄣ佺紪闃熸槸鍚﹀氨缁佹槸鍚﹀洜鏁伴噺涓嶇鎻愬墠 return锛堝鑷翠笉涓嬪彂閫熷害锛夈 + if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300) { - if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum) - return; + _sendMotionDbgLast = DateTime.Now; + DLog.Log( + $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " + + $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " + + $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " + + $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}", + "FleetCrabDbg"); } - PilotDefinition.Self.MultiVehicleAutoVx = speed; - PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh; - PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh; + if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum) + return; + + self.MultiVehicleAutoVx = speed; + self.MultiVehicleAutoFrontTh = frontTh; + self.MultiVehicleAutoRearTh = rearTh; + // D: 閫忎紶璺緞鎺у埗鍣ㄧ畻鍑虹殑鐞嗘兂杞﹂槦涓績浣嶅Э锛堟鍓嶈涓㈠純锛夛紝渚涘悇杞︽寜 layout 鍋氬墠棣堛 + self.MultiVehicleAutoIdealX = idealPos.X; + self.MultiVehicleAutoIdealY = idealPos.Y; + self.MultiVehicleAutoIdealTh = idealAngle; + self.MultiVehicleAutoHasIdeal = true; + // B: 鏍囪鍛戒护鏂伴矞搴︺傝矾寰勭粨鏉/鏃╅/鍗￠】涓嶅啀鍒锋柊姝ゆ椂鍒 鈫 涓昏溅瓒呮椂鍚庢竻闆堕熷害锛岄伩鍏嶆粦琛屻 + self.MultiVehicleAutoCmdTime = DateTime.Now; }, - MultiVehicleGetFleetPos = () => new Location + // G: 璇诲彇杞﹂槦涓績鍘熷瓙蹇収锛岄伩鍏嶈法绾跨▼璇诲埌鎾曡鐨 x/y/th 缁勫悎銆 + MultiVehicleGetFleetPos = () => { - x = PilotDefinition.Self.CenterX, - y = PilotDefinition.Self.CenterY, - th = PilotDefinition.Self.CenterTh, - l_step = 1, - tick = DateTime.Now.Ticks + var snap = PilotDefinition.Self.GetFleetCenterSnapshot(); + return new Location + { + x = snap.X, + y = snap.Y, + th = snap.Th, + l_step = 1, + tick = DateTime.Now.Ticks + }; } }; } diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index f3693ac..abcbdae 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -11,7 +11,6 @@ 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; @@ -211,42 +210,6 @@ namespace MultiWheelC 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 { diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index 5b2e603..c4b2bae 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -7,8 +7,10 @@ using ClumsyCore.Pilot; using FundamentalLib; using CommonUsage.Chassis; using CommonUsage.Mathematics; +using MDCSToolBox.Clumsy.MotionControllers; using MDCSToolBox.Clumsy.Movements; using MDCSToolBox.Clumsy.Pilot; +using MDCSToolBox.Clumsy.Tracks; namespace MultiWheelC; @@ -124,6 +126,473 @@ public class MultiRotateMovementTest : MovementTest } } +// ===== 杞﹂槦鑱斿姩-鍘熷湴鏃嬭浆鍔ㄤ綔 ===== +// 绛変环浜 FleetRemote 鐨勩屽師鍦版棆杞嶆ā寮忥紙宸插疄娴嬪彲鐢級锛欶leetRemote 閫氳繃 Medulla 鎵嬪姩 IO +// (MultiVehicleManualEnabled + Mode=2 + Vth) 椹卞姩 PilotDefinition.TickMultiVehicle 缁曡溅闃熶腑蹇冩棆杞 +// 鎵嬪姩 IO 鏄 [AsLowerIO]锛圡edulla鈫扖lumsy锛屾瘡鍛ㄦ湡鍥炲啓锛夛紝Clumsy 渚у姩浣滅洿鎺ュ啓浼氳瑕嗙洊锛 +// 鍥犳鏈姩浣滄敼鐢 Clumsy 鍐呴儴鑴氭湰瀛楁 MultiVehicleScript*锛圱ickMultiVehicle 宸插皢鍏朵綔涓烘墜鍔ㄧ瓑浠疯緭鍏ワ級锛 +// 涓嶅啓涓琛屽簳鐩樻寚浠も斺斿疄闄呯殑 SendRotateMotion + PI 绾犲亸 + 鍚戜粠杞﹀箍鎾潎鐢 TickMultiVehicle 瀹屾垚銆 +// +// 鍓嶆彁锛氬湪銆屼富杞︺(MultiVehicleMasterEndpoint == "/") 鐨 Clumsy 涓婅繍琛岋紝涓斾粠杞﹀凡娉ㄥ唽(缂栭槦灏辩华)銆 +// 鍋滄鏉′欢锛氫富杞 SLAM 鏈濆悜绱杞繃 |TargetDeltaDeg|锛堝垰浣撳師鍦版棆杞紝鏁磋溅鏈濆悜鍙樺寲閲 == 杞﹂槦杞锛夛紱 +// 鏃犲畾浣嶆椂閫鍖栦负鎸 |TargetDeltaDeg| / |Omega| 浼扮畻鏃堕暱锛涘苟甯﹀畨鍏ㄨ秴鏃躲 +public class FleetRotateInPlace : MovementDefinition +{ + /// 瑙掗熷害澶у皬(deg/s)锛涘疄闄呮柟鍚戠敱 TargetDeltaDeg 鐨勭鍙峰喅瀹氥 + public float Omega = 15f; + + /// 鐩爣鐩稿杞(deg锛屽甫绗﹀彿锛+ 涓洪嗘椂閽)銆 + public float TargetDeltaDeg = 90f; + + /// 鍒颁綅瑙掑害绮惧害(deg)銆 + public float ArriveDeg = 1.5f; + + /// 鍑忛熷尯瀹藉害(deg)锛氬墿浣欒搴﹀皬浜庢鍊兼椂锛岃閫熷害鎸夊墿浣欐瘮渚嬬嚎鎬ч檷鍒 MinOmega锛屾姂鍒舵儻鎬ц秴璋冦 + public float SlowDeg = 25f; + + /// 鍑忛熷尯鏈鏈灏忚閫熷害(deg/s)锛氶伩鍏嶈秺鎺ヨ繎鐩爣瓒婃參銆侀暱灏惧仠涓嶄笅/鍒颁笉浜嗕綅銆 + public float MinOmega = 3f; + + /// 缂撳惎鍔ㄨ鍔犻熷害(deg/s虏)锛氳捣姝ユ椂瑙掗熷害浠 0 鎸夋鏂滅巼鐖崌鍒板贰鑸硷紝鎶戝埗璧锋鎶栧姩/闃熷舰楠ゅ亸銆備粎浣滅敤浜庤捣姝ュ姞閫燂紝<=0 鍏抽棴缂撳惎鍔紙闃惰穬璧锋锛夈 + public float AccelDegPerSec2 = 20f; + + /// + /// 鏄惁鐢 Detour 涓昏溅鑸悜闂幆鍒ゅ仠锛堣 getCartLocation().th 绱瀹為檯杞锛屽埌 |TargetDeltaDeg| 鍋滐級銆 + /// 涓 MultiVehicleSyncUseDetour 瑙ｈ︼細杞埌鎸囧畾瑙掑害闇瑕佽搴﹀弽棣堬紝鏁呴粯璁 true銆 + /// false 鏃堕鍖栦负鎸変及绠楁椂闀垮紑鐜仠姝紙瀹為檯杞熲墵鎸囦护鏃朵笉绮剧‘锛夈傛敞鎰 true 鏃惰嫢鏃犳湁鏁堝叏灞瀹氫綅锛 + /// getCartLocation() 浼氶樆濉烇紙涓庡崟杞 MultiRotateToWorldAngle 琛屼负涓鑷达級銆 + /// + public bool UseDetourHeading = true; + + // 娉細涓嶈瓒呮椂涓婇檺鈥斺旀棆杞寔缁埌鍒颁綅锛堟垨鏃犲畾浣嶆椂鎸変及绠楁椂闀跨粨鏉燂級锛屾垨琚 Stop()/TestStop() 涓诲姩涓銆 + + /// 鍒颁綅鍚庝繚鎸佽剼鏈娇鑳姐佽閫熷害褰掗浂鐨勫畨瀹氭椂闀(s)锛岃绾犲亸鎶婇槦褰㈢ǔ浣忓啀鎾ょ銆 + public float SettleSec = 0.5f; + + private void ClearScript() + { + var self = PilotDefinition.Self; + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptVth = 0; + self.MultiVehicleScriptEnabled = false; + } + + public void Stop() => ClearScript(); + + public override IEnumerable Get() + { + var self = PilotDefinition.Self; + var conf = PilotDefinition.Conf; + + if (conf.MultiVehicleMasterEndpoint != "/") + { + Hedingben.ToastText("杞﹂槦鍘熷湴鏃嬭浆闇鍦ㄤ富杞(涓昏溅绔偣=\"/\")杩愯", "FleetRotate"); + yield break; + } + + var dir = Math.Sign(TargetDeltaDeg); + if (dir == 0) dir = 1; + var maxOmega = Math.Abs(Omega); + var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 鏈灏忎笉瓒呰繃鏈澶 + var slowDeg = Math.Max(1e-3f, SlowDeg); // 鍑忛熷尯瀹藉害 + var accel = AccelDegPerSec2; // 缂撳惎鍔ㄨ鍔犻熷害锛屼粎浣滅敤浜庤捣姝ワ紝<=0 鍏抽棴 + var targetMag = Math.Abs(TargetDeltaDeg); + + var hasPos = UseDetourHeading; + var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f; + var startTh = prevTh; + var accumulated = 0f; // 绱甯︾鍙疯浆瑙(deg) + var start = DateTime.Now; + var lastTime = start; + var lastLog = DateTime.MinValue; + var cmdMag = 0f; // 褰撳墠瀹為檯涓嬪彂瑙掗熷害澶у皬(deg/s)锛岀紦鍚姩浠 0 鏂滃潯鐖崌 + // 鏃犲畾浣嶆寜鏃堕暱浼扮畻鏃讹紝琛ヤ笂缂撳惎鍔ㄦ枩鍧″皯杞殑绛夋晥鏃堕棿(鈮 maxOmega/(2路accel))锛屼娇鏃堕暱鏇存帴杩戠洰鏍囪銆 + var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0; + if (accel > 1e-3) estDuration += maxOmega / (2 * accel); + + DLog.Log( + $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " + + $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " + + $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}", + "FleetRotateDbg"); + + // 浣胯兘鑴氭湰椹卞姩鐨勫師鍦版棆杞紙mode2锛夈俆ickMultiVehicle 鍚庡彴寰幆鎹鎵ц鏃嬭浆骞跺箍鎾粰浠庤溅銆 + // 璧锋浠 0 瑙掗熷害寮濮嬶紝鐢辩紦鍚姩鏂滃潯鐖崌锛岄伩鍏嶉樁璺冧笅鍙戝鑷撮槦褰㈤鍋/鎶栧姩銆 + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptMode = 2; + self.MultiVehicleScriptVth = 0; + self.MultiVehicleScriptEnabled = true; + + var stopReason = "stop()"; + while (true) + { + var now = DateTime.Now; + var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds)); + lastTime = now; + var elapsed = (now - start).TotalSeconds; + + float desiredMag; + float curTh = 0f, remaining = 0f, actualRate = 0f; + if (hasPos) + { + curTh = (float)DetourInterface.getCartLocation().th; + var step = (float)CommonMath.ThDiff(curTh, prevTh); // 鏈抚瀹為檯杞(閫嗘椂閽堜负姝) + accumulated += step; + actualRate = dt > 1e-3 ? step / dt : 0f; // 瀹為檯瑙掗熺巼(deg/s)锛岀敤浜庡姣旀寚浠 + prevTh = curTh; + + remaining = targetMag - Math.Abs(accumulated); + if (remaining <= ArriveDeg) { stopReason = "arrived"; break; } + + // 鍑忛熷尯锛氬墿浣欒搴 < SlowDeg 鏃讹紝鐩爣瑙掗熷害鎸夊墿浣欐瘮渚嬬嚎鎬ч檷鍒 MinOmega锛 + // 浣垮垏鏂寚浠ょ灛闂存畫浣欏姩閲忚冻澶熷皬锛屾姂鍒舵儻鎬ф粦琛岄犳垚鐨勮秴璋冦傚搴︾洿瑙傘佷究浜庣幇鍦鸿皟璇曘 + desiredMag = remaining < slowDeg + ? Math.Max(minOmega, maxOmega * (remaining / slowDeg)) + : maxOmega; + } + else + { + // 鏃犲畾浣嶏細鎸夋椂闀夸及绠楋紝鏃犳硶娴嬭锛岀洰鏍囩淮鎸佸贰鑸熷害鍒颁及绠楁椂闀匡紙浠呯紦鍚姩鏁村舰锛夈 + desiredMag = maxOmega; + if (elapsed >= estDuration) { stopReason = "estDuration"; break; } + } + + // 缂撳惎鍔細鍙鈥滃姞閫(鐩爣>褰撳墠)鈥濇寜瑙掑姞閫熷害闄愭枩鐜囷紝璁╄捣姝ュ钩婊戠埇鍗囷紱 + // 鈥滃噺閫(鐩爣<褰撳墠)鈥濊窡闅忎笂闈㈢殑鍑忛熸洸绾跨珛鍗充笅璋冿紝淇濊瘉鍙婃椂鍒硅溅涓嶈秴璋冦 + if (accel > 1e-3 && desiredMag > cmdMag) + cmdMag = Math.Min(desiredMag, cmdMag + accel * dt); + else + cmdMag = desiredMag; + + self.MultiVehicleScriptVth = dir * cmdMag; + + // 钀界洏璇婃柇(鑺傛祦~150ms)锛氬疄闄呰埅鍚/绱杞/瀹為檯瑙掗熺巼 vs 鎸囦护瑙掗熺巼锛屽畾浣"寮鐜浆閫熶笉瓒"銆 + if ((now - lastLog).TotalMilliseconds >= 150) + { + lastLog = now; + DLog.Log( + hasPos + ? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " + + $"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (瀹為檯/鎸囦护={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})" + : $"t={elapsed:0.00}s/{estDuration:0.00}s (鏃犺埅鍚戝弽棣,寮鐜寜鏃堕暱) cmdW={dir * cmdMag:0.0}", + "FleetRotateDbg"); + } + + Hedingben.ToastText( + hasPos + ? $"杞﹂槦鍘熷湴鏃嬭浆 鐩爣{TargetDeltaDeg:0.0}掳 宸茶浆{accumulated:0.0}掳 浣檣targetMag - Math.Abs(accumulated):0.0}掳 蠅={cmdMag:0.0}" + : $"杞﹂槦鍘熷湴鏃嬭浆(鏃犲畾浣,鎸夋椂闀) {elapsed:0.0}/{estDuration:0.0}s 蠅={cmdMag:0.0}", + "FleetRotate"); + + yield return true; + } + + // 鍒颁綅锛氳閫熷害鍏堝綊闆讹紝淇濇寔鑴氭湰浣胯兘璁 TickMultiVehicle 鐨 PI 鎶婇槦褰㈢ǔ浣忎竴灏忔鏃堕棿鍐嶆挙绂汇 + self.MultiVehicleScriptVth = 0; + var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec)); + while (DateTime.Now < settleEnd) + yield return true; + + ClearScript(); + DLog.Log( + $"DONE reason={stopReason} 绱杞={accumulated:0.0}掳 鐩爣={TargetDeltaDeg:0.0}掳 " + + $"鐢ㄦ椂={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}", + "FleetRotateDbg"); + Hedingben.ToastText($"杞﹂槦鍘熷湴鏃嬭浆瀹屾垚({stopReason}) 绱{accumulated:0.0}掳", "FleetRotate"); + } +} + +[MovementTest(name = "杞﹂槦鑱斿姩-鍘熷湴鏃嬭浆")] +public class FleetRotateInPlaceTest : MovementTest +{ + private FleetRotateInPlace _proc; + private DriveTask _task; + + public override void Test() + { + _proc = new FleetRotateInPlace + { + Omega = PilotDefinition.Conf.FleetRotateOmega, + TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg, + ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg, + SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg, + MinOmega = PilotDefinition.Conf.FleetRotateMinOmega, + AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel, + SettleSec = PilotDefinition.Conf.FleetRotateSettleSec, + UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading + }; + _task = new DriveTask(_proc.Get()); + _task.Wait(); + } + + public override void TestStop() + { + _proc?.Stop(); + _task?.Stop(); + } +} + +// ===== 杞﹂槦鑱斿姩-鑷姩锜硅鍔ㄤ綔 ===== +// 浠ュ綋鍓嶈溅闃熶腑蹇冧负璧风偣锛屾瀯閫犱笌杞﹂槦鏈濆悜澶硅 x銆侀暱搴 y 鐨勭洿绾胯矾寰勶紱鎵ц渚у鐢 +// TickMultiVehicle 鐨勮剼鏈墜鍔ㄧ瓑浠疯緭鍏(mode=1)锛屼篃灏辨槸 FleetRemote 鎵嬪姩锜硅鍚屼竴鏉′笅鍙戦摼璺 +// +// 鎵嬪姩锜硅宸查獙璇佷笣婊戯紝鑷姩鍔ㄤ綔鍙澶栧仛涓や欢浜嬶細 +// 1) 璇诲彇涓昏溅 Detour 鍙嶆帹杞﹂槦涓績锛岃绠楁部鐩寸嚎鐨勮繘搴﹀拰妯悜鍋忓樊锛 +// 2) 鐢ㄥ皬骞呫佸甫鏂滅巼闄愬埗鐨勬柟鍚戜慨姝ｅ啓 MultiVehicleScriptVx/Vy锛岄伩鍏嶅嚑浣曟帶鍒跺櫒 bias/dTh 闃惰穬閫犳垚鎶栧姩銆 +// +// 涓 FleetRemote 鎵嬪姩锜硅(mode==1)瀵圭収锛歍ickMultiVehicle 浠嶈礋璐ｅ悎鎴 frontTh==rearTh 鐨勮煿琛岃埖瑙掑苟骞挎挱缁欎粠杞︺ +// +// 鍓嶆彁锛氬湪涓昏溅(MultiVehicleMasterEndpoint=="/")杩愯锛屼笖涓昏溅鏈 Detour 瀹氫綅銆 +public class FleetCrabWalk : MovementDefinition +{ + /// 涓庡綋鍓嶈溅闃熸湞鍚戠殑澶硅(deg锛岄嗘椂閽堜负姝)銆 + public float CrabAngleDeg = 45f; + + /// 璺緞闀垮害(mm)銆 + public float CrabLengthMm = 2000f; + + /// 琛岄┒閫熷害(m/s)銆 + public float CrabSpeed = 0.2f; + + /// 鍏煎鏃ч厤缃紱褰撳墠鑴氭湰鎵嬪姩绛変环瀹炵幇涓嶅啀鐩存帴浣跨敤鍑犱綍鎺у埗鍣 gcp 涓婇檺銆 + public float GcpThetaThreshold = 95f; + + /// 妯悜璇樊杞悜澧炵泭锛屾部鐢 Stanley 褰㈠紡锛歛tan(gain * lateral / speed)銆 + public float CorrectionGain = 1f; + + /// 鑷姩绾犲亸鏈澶ф敼鍚戣(deg)銆傝秺灏忚秺鎺ヨ繎鎵嬪姩锜硅锛岃秺澶ф敹鏁涜秺蹇 + public float CorrectionAngleThreshold = 8f; + + /// 鑴氭湰 Vx/Vy 鍛戒护鏂滅巼闄愬埗(m/s^2)锛岄伩鍏嶇籂鍋忛噺鍙樺寲閫犳垚鑸佃闃惰穬銆 + public float CommandAccel = 0.4f; + + private bool _stopping; + + private void Cleanup() + { + var self = PilotDefinition.Self; + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptVth = 0; + self.MultiVehicleScriptMode = 0; + self.MultiVehicleScriptEnabled = false; + self.MultiVehicleAutoVx = 0; + self.MultiVehicleAutoFrontTh = 0; + self.MultiVehicleAutoRearTh = 0; + self.MultiVehicleAutoHasIdeal = false; + self.MultiVehicleAutoEnabled = false; + } + + public void Stop() + { + _stopping = true; + Cleanup(); + } + + private static float Slew(float current, float target, float maxStep) + { + var diff = target - current; + if (Math.Abs(diff) <= maxStep) return target; + return current + Math.Sign(diff) * maxStep; + } + + public override IEnumerable Get() + { + var self = PilotDefinition.Self; + var conf = PilotDefinition.Conf; + _stopping = false; + + DLog.Log( + $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " + + $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " + + $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " + + $"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}", + "FleetCrabDbg"); + + if (conf.MultiVehicleMasterEndpoint != "/") + { + DLog.Log($"ABORT: 闈炰富杞 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg"); + Hedingben.ToastText("杞﹂槦锜硅闇鍦ㄤ富杞(涓昏溅绔偣=\"/\")杩愯", "FleetCrab"); + yield break; + } + + // 娉ㄦ剰锛歡etCartLocation() 鍦ㄦ棤鏈夋晥 Detour 瀹氫綅鏃朵細闃诲鈥斺旇嫢鍗″湪杩欓噷涓斿悗闈㈢湅涓嶅埌 CENTER 鏃ュ織锛屽嵆瀹氫綅鏈氨缁 + DLog.Log("涓昏溅鏍￠獙閫氳繃锛屽紑濮嬭鍙栬溅闃熶腑蹇 (getCartLocation 鏃犲畾浣嶄細闃诲)鈥", "FleetCrabDbg"); + if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta)) + { + DLog.Log("ABORT: TryGetFleetCenterFromSlam 杩斿洖 false (鏃犲畾浣)", "FleetCrabDbg"); + Hedingben.ToastText("杞﹂槦锜硅闇瑕佷富杞 Detour 瀹氫綅", "FleetCrab"); + yield break; + } + DLog.Log($"CENTER 杞﹂槦涓績=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg"); + + var phi = CommonMath.RoundTh(theta + CrabAngleDeg); + var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0)); + var phiRad = phi / 180.0 * Math.PI; + var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad)); + var pathLeft = new Vector2(-pathDir.Y, pathDir.X); + + DLog.Log( + $"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " + + $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}", + "FleetCrabDbg"); + + // 鎵嬪姩锜硅宸查獙璇佷笣婊戯細杩欓噷澶嶇敤 TickMultiVehicle 鐨勮剼鏈墜鍔ㄧ瓑浠疯緭鍏(mode=1)锛 + // 鍙湪鏈姩浣滃唴鏍规嵁杞﹂槦涓績鐩稿鐩寸嚎鐨勬í鍚戣宸紦鎱㈣皟鏁 Vx/Vy 鏂瑰悜銆 + self.MultiVehicleScriptEnabled = true; + self.MultiVehicleScriptMode = 1; + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptVth = 0; + self.PrimeMasterAutoFromSlam(); + DLog.Log("WARMUP 宸插惎鐢ㄨ剼鏈煿琛(mode=1)锛岀瓑寰呯紪闃熸垚鍛樺氨浣嶁", "FleetCrabDbg"); + + var warmEnd = DateTime.Now.AddSeconds(2.0); + var warmIter = 0; + var warmReady = false; + while (!_stopping && DateTime.Now < warmEnd) + { + warmIter++; + self.MultiVehicleScriptEnabled = true; + self.MultiVehicleScriptMode = 1; + self.PrimeMasterAutoFromSlam(); + var snap = self.GetFleetCenterSnapshot(); + int cnt; + lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count; + if (warmIter % 5 == 0) + DLog.Log( + $"WARMUP#{warmIter} 蹇収=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " + + $"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}", + "FleetCrabDbg"); + if (cnt >= conf.MultiVehicleFleetNum) + { + warmReady = true; + DLog.Log( + $"WARMUP done iter={warmIter} 蹇収=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}", + "FleetCrabDbg"); + break; + } + yield return true; + } + if (!warmReady) + DLog.Log("WARMUP 瓒呮椂锛氱紪闃熶粛鏈氨浣嶏紝缁х画杩涘叆鑴氭湰锜硅锛堣嫢涓嶅姩璇锋煡鐪 FleetDiagClumsy ready/cnt锛", + "FleetCrabDbg"); + + Hedingben.ToastText($"杞﹂槦锜硅 澶硅{CrabAngleDeg:0.0}掳 闀垮害{CrabLengthMm:0}mm", "FleetCrab"); + + var iter = 0; + var cmdVx = 0f; + var cmdVy = 0f; + var lastTime = DateTime.Now; + var lastLog = DateTime.MinValue; + var finishDistance = Math.Max(20f, conf.FinishDistance); + var stopReason = "done"; + + while (!_stopping) + { + iter++; + var now = DateTime.Now; + var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds)); + lastTime = now; + + self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth); + var delta = new Vector2(cx - x0, cy - y0); + var along = Vector2.Dot(delta, pathDir); + var lateral = Vector2.Dot(delta, pathLeft); + var remain = CrabLengthMm - along; + if (remain <= finishDistance) + break; + + var speed = Math.Abs(CrabSpeed); + if (remain < conf.SlowDistance && conf.SlowDistance > 1) + { + var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow); + speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed); + } + + var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0); + correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold)); + var desiredWorldAngle = CommonMath.RoundTh(phi + correction); + var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth); + var localRad = localAngle / 180.0 * Math.PI; + var targetVx = speed * (float)Math.Cos(localRad); + var targetVy = speed * (float)Math.Sin(localRad); + var maxStep = Math.Max(0.05f, CommandAccel) * dt; + cmdVx = Slew(cmdVx, targetVx, maxStep); + cmdVy = Slew(cmdVy, targetVy, maxStep); + + self.MultiVehicleScriptEnabled = true; + self.MultiVehicleScriptMode = 1; + self.MultiVehicleScriptVx = cmdVx; + self.MultiVehicleScriptVy = cmdVy; + self.MultiVehicleScriptVth = 0; + + if ((DateTime.Now - lastLog).TotalMilliseconds >= 300) + { + lastLog = DateTime.Now; + var snap = self.GetFleetCenterSnapshot(); + int fleetCnt; + lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count; + DLog.Log( + $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " + + $"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " + + $"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}", + "FleetCrabDbg"); + } + yield return true; + } + + if (_stopping) + stopReason = "stop"; + + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptVth = 0; + var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3)); + while (!_stopping && DateTime.Now < settleEnd) + { + self.MultiVehicleScriptEnabled = true; + self.MultiVehicleScriptMode = 1; + yield return true; + } + + Cleanup(); + Hedingben.ToastText("杞﹂槦锜硅瀹屾垚", "FleetCrab"); + DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg"); + } +} + +[MovementTest(name = "杞﹂槦鑱斿姩-鑷姩锜硅")] +public class FleetCrabWalkTest : MovementTest +{ + private FleetCrabWalk _proc; + private DriveTask _task; + + public override void Test() + { + _proc = new FleetCrabWalk + { + CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg, + CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm, + CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed, + GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold, + CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain, + CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg, + CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel + }; + _task = new DriveTask(_proc.Get()); + _task.Wait(); + } + + public override void TestStop() + { + _proc?.Stop(); + _task?.Stop(); + } +} + // ===== 璋冪敤 Playground WebAPI 鐬Щ灏忚溅锛堝墠绉 / 宸︾Щ / 鏃嬭浆锛===== // 骞崇Щ/鏃嬭浆閲忓湪 Fields 闈㈡澘閰嶇疆锛歐ebApiTranslateMm锛堥粯璁100mm锛夈乄ebApiRotateDeg锛堥粯璁5搴︼級銆 diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 37fdb21..b0abcad 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -16,7 +16,6 @@ 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; @@ -74,55 +73,6 @@ namespace MultiWheelC } - 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; diff --git a/MultiWheel/MultiWheelC/MultiWheelC.csproj b/MultiWheel/MultiWheelC/MultiWheelC.csproj index 8962a0d..a279414 100644 --- a/MultiWheel/MultiWheelC/MultiWheelC.csproj +++ b/MultiWheel/MultiWheelC/MultiWheelC.csproj @@ -10,8 +10,6 @@ - - diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index b5d93db..a188c88 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -14,8 +14,14 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "[sync] 鎵嬪姩Vx绯绘暟")] public float ManualCarSyncVxFac = 1f; [FieldMember(desc = "[sync] 鎵嬪姩Vy绯绘暟(锜硅妯悜)")] public float ManualCarSyncVyFac = 1f; [FieldMember(desc = "[sync] 鎵嬪姩Vth绯绘暟")] public float ManualCarSyncVthFac = 1f; + [FieldMember(desc = "[sync] 锜硅鑸佃涓婇檺(deg锛屽簲涓嶮edulla鑸佃疆瑙掑害闄愬埗鍖归厤锛岄粯璁120)")] public float MultiVehicleCrabSteerLimitDeg = 120f; [FieldMember(desc = "[sync] 妫娴嬩腑蹇冨亸绉(mm)")] public float DeltaDetectCenter = 350f; - [FieldMember(desc = "[sync] 鏄惁鍚敤Detour瀹氫綅")] public bool PosAvailable = true; + // 浠呮帶鍒"杞﹂槦鍐呭Э鎬佺籂姝"(POS 琛ュ伩)鏄惁浣跨敤 Detour 鐨 SLAM 浣嶅Э锛屼笉褰卞搷"鏁翠釜杞﹂槦濮挎佺殑璁＄畻"銆 + // 榛樿 false锛氬畾浣嶄笉鍙備笌杞﹂槦鍐呭Э鎬佺籂姝ｏ紙鍚勮溅鎸夌紪闃熷嚑浣/浜掕瘑鍒繚鎸侀槦褰紝涓嶅仛 SLAM 閫愯溅绾犲亸锛夈 + // 涓 true锛氶澶栫敤 getCartLocation() 鍙嶆帹姣忓彴杞︾浉瀵圭紪闃熶腑蹇冪殑鍋忓樊骞跺仛 POS 琛ュ伩銆 + // 娉ㄦ剰锛氭棤璁鸿寮鍏冲浣曪紝鑷姩妯″紡涓嬫暣闃熷Э鎬(鍙嶆帹/骞挎挱杞﹂槦涓績銆丼LAM 闂磋窛銆佽嚜鍔ㄥ畨鍏ㄩ棬)濮嬬粓渚濊禆 Detour 鍏ㄥ眬瀹氫綅锛 + // 涓昏溅鑷姩妯″紡蹇呰皟鐢 getCartLocation()锛岃嫢鏃犳湁鏁堝叏灞瀹氫綅璇ヨ皟鐢ㄤ細闃诲 鈫 鑱斿姩绾跨▼闃诲涓嶄笅鍙戦熷害锛堝畨鍏ㄥ仠杞︼級銆 + [FieldMember(desc = "[sync] 瀹氫綅鏄惁鍙備笌杞﹂槦鍐呭Э鎬佺籂姝(涓嶅奖鍝嶆暣闃熷Э鎬佽绠)")] public bool MultiVehicleSyncUseDetour = false; [FieldMember(desc = "澶氳溅鑱斿姩锛氭昏溅鏁")] public int MultiVehicleFleetNum = 2; [FieldMember(desc = "鑱斿姩绾跨▼鍛ㄦ湡(ms)")] public int MultiVehicleSyncInterval = 50; @@ -34,6 +40,21 @@ public class PilotConfig : MultiWheelPilotConfig } [FieldMember(desc = "澶氳溅鑱斿姩锛氬惎鐢ㄤ簰璇嗗埆绾犳")] public bool MultiVehicleUseDetect = false; + + // E: 缂栭槦鎺у埗鐐瑰崐寰(mm)銆0 琛ㄧず鑷姩鍙 syncDistance/2锛堜笌 SetOriginBias 鍑犱綍涓鑷达級锛>0 鏃舵寜鏈煎浐瀹氥 + // 鍙栦唬鍘嗗彶纭紪鐮 510锛岄伩鍏嶆敼闂磋窛鍚庢帶鍒剁偣鍗婂緞涓嶈窡闅忓鑷磋浆鍚/琛ュ伩鍑犱綍閿欎綅銆 + [FieldMember(desc = "澶氳溅鑱斿姩锛氭帶鍒剁偣鍗婂緞(mm锛0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f; + // B: 鑷姩閫熷害鍛戒护鏂伴矞搴(ms)銆備富杞﹁秴杩囨鏃堕暱鏈粠璺緞鎺у埗鍣ㄦ敹鍒版柊閫熷害鍛戒护(璺緞缁撴潫/鏃╅/鍗￠】)锛 + // 鍗宠涓哄け鏁堝苟娓呴浂涓嬪彂閫熷害锛岄伩鍏嶈溅闃熸寜鏈熷害婊戣銆0 琛ㄧず鑷姩鍙 max(200, interval*4)銆 + [FieldMember(desc = "澶氳溅鑱斿姩锛氳嚜鍔ㄩ熷害鍛戒护瓒呮椂(ms锛0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0; + // C: fleet 鎴愬憳瀛樻椿 TTL(ms)銆備富杞﹀墧闄よ秴杩囨鏃堕暱鏈 register/鍒锋柊鐨勪粠杞︼紱缂栭槦灏辩华瑕佹眰鎵鏈夋垚鍛樻柊椴溿 + // 0 琛ㄧず鑷姩鍙 max(500, interval*6)銆 + [FieldMember(desc = "澶氳溅鑱斿姩锛氭垚鍛樺瓨娲籘TL(ms锛0=auto)")] public int MultiVehicleMemberTtlMs = 0; + // D: 鑷姩妯″紡涓嬬敤涓昏溅璺緞鎺у埗鍣ㄧ殑鐞嗘兂杞﹂槦涓績(idealPos/idealAngle)浣滀负鍚勮溅 layout 鐩爣锛 + // 寮х嚎璺緞涓婂仛 per-car 鍓嶉鑰岄潪浠呭叡鐢 frontTh/rearTh 浜嬪悗绾犲亸銆 + [FieldMember(desc = "澶氳溅鑱斿姩锛氳嚜鍔ㄦā寮忔寜鐞嗘兂涓績鍓嶉(寮х嚎)")] public bool MultiVehicleAutoUseIdealCenter = true; + // H: 鑷姩妯″紡蹇呴』鏈夋湁鏁堣溅闃熶腑蹇(SLAM 鍙弽鎺)锛屽叏绋嬪畾浣嶄涪澶辨椂鍋滆溅锛岄伩鍏嶇函 SLAM 涓嬬洸璺戙 + [FieldMember(desc = "澶氳溅鑱斿姩锛氳嚜鍔ㄦā寮忚姹傛湁鏁堣溅闃熶腑蹇")] public bool MultiVehicleAutoRequireFleetCenter = true; [FieldMember(desc = "澶氳溅鑱斿姩锛歋LAM X琛ュ伩绯绘暟")] public float MultiVehiclePosBiasXFac = 0.5f; [FieldMember(desc = "澶氳溅鑱斿姩锛歋LAM Y琛ュ伩绯绘暟")] public float MultiVehiclePosBiasYFac = 0.5f; [FieldMember(desc = "澶氳溅鑱斿姩锛歋LAM Th琛ュ伩绯绘暟")] public float MultiVehiclePosBiasThFac = 0.5f; @@ -61,6 +82,10 @@ public class PilotConfig : MultiWheelPilotConfig // 浠呭綋杞﹂槦瀹為檯琚寚浠ゆ棆杞(|fleetOmega|瓒呰繃姝ら槇鍊)鏃舵墠杩愯绾犲亸 PI锛涘惁鍒欐竻闆跺苟澶嶄綅绉垎锛 // 閬垮厤鏉惧紑鎽囨潌鍚庣Н鍒嗘畫鐣欐寔缁┍鍔ㄨ溅杈"鑷鏃嬭浆鍋滀笉涓嬫潵"銆 [FieldMember(desc = "鍘熷湴鏃嬭浆绾犲亸锛氱敓鏁堢殑鏈灏忚閫熷害闃堝(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f; + // 鍙夌‖瀹夊叏缃戯細姣忚疆绾犲亸閫熷害骞呭 鈮 璇ユ瘮渚嬅楁湰杞棆杞垏鍚戦熷害锛岄檺鍒跺悎閫熷害鐩稿绾垏鍚戠殑鏈澶у亸瑙掋 + // 榛樿 <0 鍏抽棴鈥斺旂籂鍋忛殢杞熺缉鏀(浠ｇ爜 #1)宸茶"绾犲亸:鍒囧悜"姣斾緥鍏ㄧ▼鎭掑畾锛屽寑閫熸涓嶅簲鍐嶈鍓婂急銆 + // 浠呭湪鏋佺鍚姩鍋忓樊瀵艰嚧鍖閫熸浠嶄贡鎵撴柟鍚戞椂锛屽彲璁句负 ~1.0(鍋忚鈮45掳) 鍏滃簳銆 + [FieldMember(desc = "鍘熷湴鏃嬭浆绾犲亸锛氱籂鍋/鏃嬭浆鍒囧悜姣斾緥纭笂闄(榛樿-1鍏抽棴)")] public float MultiVehicleRotateCompTangentFrac = -1f; [FieldMember(desc = "鍗曡溅鍚屾 xy 绮惧害(mm)")] public float SingleCarSyncPrecisionXy = 10f; [FieldMember(desc = "鍗曡溅鍚屾 th 绮惧害(deg)")] public float SingleCarSyncPrecisionTh = 0.2f; @@ -92,6 +117,59 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "鍘熷湴鏃嬭浆锛氳捣杞墠鑸佃疆瀵归綈绮惧害(deg)")] public float InPlaceRotateWheelAlignDeg = 2f; + // ===== 杞﹂槦鑱斿姩-鍘熷湴鏃嬭浆鍔ㄤ綔锛團leetRotateInPlace / 瀵瑰簲 FleetRemote 鍘熷湴鏃嬭浆妯″紡锛===== + // 閫氳繃 Clumsy 鍐呴儴鑴氭湰瀛楁椹卞姩 TickMultiVehicle 鐨 mode2 鏃嬭浆锛堢粫杞﹂槦涓績 + PI 绾犲亸锛夛紝闇涓昏溅杩愯銆 + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氳閫熷害澶у皬(deg/s锛屾柟鍚戠敱鐩爣瑙掔鍙峰喅瀹)")] + public float FleetRotateOmega = 15f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氱洰鏍囩浉瀵硅浆瑙(deg锛+閫嗘椂閽)")] + public float FleetRotateTargetDeltaDeg = 90f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氬埌浣嶈搴︾簿搴(deg)")] + public float FleetRotateArriveDeg = 1.5f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氬噺閫熷尯瀹藉害(deg)锛屾姂鍒舵敹灏炬儻鎬ц秴璋")] + public float FleetRotateSlowDeg = 25f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氬噺閫熷尯鏈鏈灏忚閫熷害(deg/s)")] + public float FleetRotateMinOmega = 3f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氳捣姝ョ紦鍚姩瑙掑姞閫熷害(deg/s虏锛<=0鍏抽棴)")] + public float FleetRotateAccel = 20f; + + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氬埌浣嶅悗瀹夊畾鏃堕暱(s)")] + public float FleetRotateSettleSec = 0.5f; + + // 涓 MultiVehicleSyncUseDetour 瑙ｈ︼細杞埌鎸囧畾瑙掑害闇鑸悜鍙嶉锛岄粯璁 true 璇讳富杞 SLAM 鑸悜闂幆鍒ゅ仠銆 + // false 鏃堕鍖栦负鎸変及绠楁椂闀垮紑鐜仠姝紙瀹為檯杞熲墵鎸囦护鏃朵笉绮剧‘锛屾槗鍑虹幇"娌¤浆鍒扮洰鏍囧氨鍋"锛夈 + [FieldMember(desc = "杞﹂槦鍘熷湴鏃嬭浆锛氱敤Detour涓昏溅鑸悜闂幆鍒ゅ仠(榛樿true锛宖alse=鎸夋椂闀垮紑鐜)")] + public bool FleetRotateUseDetourHeading = true; + + // ===== 杞﹂槦鑱斿姩-鑷姩锜硅锛團leetCrabWalk锛===== + // 浠ュ綋鍓嶈溅闃熶腑蹇冧负璧风偣锛屾瀯閫犱笌杞﹂槦鏈濆悜澶硅 FleetCrabAngleDeg銆侀暱搴 FleetCrabLengthMm 鐨勭洿绾胯矾寰勶紝 + // 澶嶇敤鑴氭湰鎵嬪姩绛変环杈撳叆(mode=1)鏂滃悜骞崇Щ锛涘姩浣滀晶鍙妸妯悜璇樊杞崲鎴愬皬骞呫佸甫鏂滅巼闄愬埗鐨勮煿琛屾柟鍚戜慨姝ｃ + [FieldMember(desc = "杞﹂槦锜硅锛氫笌杞﹂槦鏈濆悜澶硅(deg锛岄嗘椂閽堜负姝)")] + public float FleetCrabAngleDeg = 45f; + + [FieldMember(desc = "杞﹂槦锜硅锛氳矾寰勯暱搴(mm)")] + public float FleetCrabLengthMm = 2000f; + + [FieldMember(desc = "杞﹂槦锜硅锛氳椹堕熷害(m/s)")] + public float FleetCrabSpeed = 0.2f; + + // 鍏煎鏃х増鍑犱綍鎺у埗鍣ㄥ疄鐜帮紱褰撳墠鑷姩锜硅璧拌剼鏈墜鍔ㄧ瓑浠疯緭鍏ワ紝涓嶅啀鐩存帴浣跨敤璇ヤ笂闄愩 + [FieldMember(desc = "杞﹂槦锜硅锛氭棫鍑犱綍鎺у埗鍣╣cp瑙掑害涓婇檺(deg)")] + public float FleetCrabGcpThetaThreshold = 95f; + + [FieldMember(desc = "杞﹂槦锜硅锛氭í鍚戣宸籂鍋忓鐩")] + public float FleetCrabCorrectionGain = 1f; + + [FieldMember(desc = "杞﹂槦锜硅锛氳嚜鍔ㄧ籂鍋忔渶澶ф敼鍚戣(deg)")] + public float FleetCrabCorrectionAngleDeg = 8f; + + [FieldMember(desc = "杞﹂槦锜硅锛氳剼鏈熷害鍛戒护鏂滅巼(m/s^2)")] + public float FleetCrabCommandAccel = 0.4f; + // ===== 2鑵挎娴嬶紙鍗曠嚎闆疯揪璇嗗埆涓よ吙鎵樼洏 / 杞儙锛===== [FieldMember(desc = "2鑵挎娴嬶細闆疯揪鍚嶏紙閫楀彿鍒嗛殧鍙涓級")] public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; @@ -152,15 +230,6 @@ public class PilotConfig : MultiWheelPilotConfig [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; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index d45e1c7..9170199 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -41,6 +41,16 @@ public class PilotDefinition : MultiWheelPilotDefinition MultiVehicleFleet = new(); public VehicleSyncNotification MultiVehicleNotification; + // A: 浜掓枼閿佸浐瀹氫负鐙珛 readonly 瀵硅薄锛岀粷涓嶉殢 MultiVehicleFleet 瀛楁琚暣浣撴浛鎹㈣屽け鏁堛 + // 鎵鏈夊 MultiVehicleFleet / _multiVehicleFleetSeen 鐨勮鍐欓兘蹇呴』 lock(FleetLock)銆 + public readonly object FleetLock = new(); + + // C: 鍚 fleet 鎴愬憳鏈杩戜竴娆¤ register/notify 鍒锋柊鐨勬湰鍦版椂鍒伙紙鐢ㄦ湰鍦版椂閽燂紝閬垮厤杩滅鏃堕挓鍋忓樊锛夈 + // 涓昏溅 Tick 鎹鍓旈櫎鎺夌嚎鎴愬憳锛涚紪闃熷氨缁姹傛墍鏈夋垚鍛樻柊椴溿 + private readonly Dictionary _multiVehicleFleetSeen = new(); + [FieldMember(desc = "澶氳溅鑱斿姩锛氳溅闃熷Э鎬亁")] public float CenterX; [FieldMember(desc = "澶氳溅鑱斿姩锛氳溅闃熷Э鎬亂")] public float CenterY; [FieldMember(desc = "澶氳溅鑱斿姩锛氳溅闃熷Э鎬乼h")] public float CenterTh; + // G: 杞﹂槦涓績鍘熷瓙蹇収銆備笁涓 float 鏃犳硶鏁翠綋鍘熷瓙鍐欙紝鏀逛负鏁翠綋鏂板缓瀵硅薄鍚庡紩鐢ㄨ祴鍊硷紙寮曠敤璧嬪煎師瀛愶級锛 + // 璺緞鎺у埗鍣ㄧ嚎绋嬪彧璇绘渶杩戜竴娆″畬鏁村揩鐓э紝閬垮厤璇诲埌 x 鏂 / y,th 鏃х殑鎾曡缁勫悎銆 + public sealed class FleetCenterSnapshot + { + public float X, Y, Th; + public long Tick; + } + private volatile FleetCenterSnapshot _fleetCenter = new(); + public FleetCenterSnapshot GetFleetCenterSnapshot() => _fleetCenter; + private void PublishFleetCenter(float x, float y, float th) + { + CenterX = x; + CenterY = y; + CenterTh = th; + _fleetCenter = new FleetCenterSnapshot { X = x, Y = y, Th = th, Tick = DateTime.Now.Ticks }; + } + + // B: 璺緞鎺у埗鍣ㄦ渶杩戜竴娆″啓鍏ヨ嚜鍔ㄩ熷害鍛戒护鐨勬椂鍒伙紱瓒呮椂鍗宠涓哄け鏁堬紙璺緞缁撴潫/鏃╅/鍗￠】锛夛紝娓呴浂涓嬪彂銆 + public DateTime MultiVehicleAutoCmdTime = DateTime.MinValue; + // D: 璺緞鎺у埗鍣ㄩ忎紶鐨勭悊鎯宠溅闃熶腑蹇冧綅濮匡紙涓栫晫绯伙級锛岀敱 idealPos/idealAngle 鍐欏叆銆 + public bool MultiVehicleAutoHasIdeal; + public float MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh; + + // F: notify 搴忓垪鍙凤紙涓昏溅鍗曡皟閫掑锛変笌浠庤溅宸插簲鐢ㄧ殑鏈澶у簭鍒楀彿锛堜涪寮冧贡搴忔棫鍖咃級銆 + private long _multiVehicleNotifySeq; + private long _multiVehicleAppliedSeq = -1; + [AsLowerIO(desc = "杞﹀彿")] public int CarNum = 1; - #region 澶硅噦鍙橀噺 - [AsUpperIO(desc = "宸﹀す鑷備笅鍙戦熷害")] public float SpeedLeftArm; - - [AsUpperIO(desc = "鍙冲す鑷備笅鍙戦熷害")] public float SpeedRightArm; - - [AsLowerIO(desc = "宸﹀す鑷傚疄闄呬綅缃")] public float ActualPosLeftArm; - - [AsLowerIO(desc = "鍙冲す鑷傚疄闄呬綅缃")] public float ActualPosRightArm; - - [AsUpperIO(desc = "澶硅噦涓嶅悓姝ユ姤璀")] public bool ClampOutOfSync = false; - #endregion - [AsLowerIO(desc = "宸﹀墠宸﹁疆瀹為檯浣嶇疆")] public float LFLActualPos; [AsLowerIO(desc = "宸﹀墠鍙宠疆瀹為檯浣嶇疆")] public float LFRActualPos; [AsLowerIO(desc = "鍙冲墠宸﹁疆瀹為檯浣嶇疆")] public float RFLActualPos; @@ -76,11 +109,6 @@ public class PilotDefinition : MultiWheelPilotDefinition(infoJson) ?? throw new Exception("VehicleSyncInfo is null"); - lock (MultiVehicleFleet) + lock (FleetLock) + { MultiVehicleFleet[query.CarNum] = info; + _multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 鍒锋柊瀛樻椿鏃跺埢 + } return JsonConvert.SerializeObject(new { code = 200, message = "ok" }); } catch (Exception e) @@ -222,16 +254,30 @@ public class PilotDefinition : MultiWheelPilotDefinition + // F: notify 鏀圭敤 POST + JSON body锛堝彇浠 GET query 涓诧級锛岄伩鍏嶆暣闃 Fleet 瀛楀吀鎾戠垎 URL 闀垮害涓婇檺锛 + // 鐢 Seq 涓㈠純涔卞簭鍒拌揪鐨勬棫鍖咃紝閬垮厤浠庤溅鐭殏濂楃敤杩囨湡鎸囦护銆 + PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body => { try { - var notifyJson = Uri.UnescapeDataString(query.Notification ?? ""); - var notification = JsonConvert.DeserializeObject(notifyJson) + var notification = JsonConvert.DeserializeObject(body ?? "") ?? throw new Exception("notification is null"); + // 涔卞簭涓㈠純锛氫粎搴旂敤搴忓垪鍙峰ぇ浜庡凡搴旂敤鍊肩殑鍖呫係eq==0 瑙嗕负鏃х増鏃犲簭鍒楀彿濮嬬粓鎺ュ彈锛 + // 鑻 Seq 鏄庢樉鍥為(宸>100)锛屽垽瀹氫负涓昏溅閲嶅惎鐨勬柊浼氳瘽锛岄噸鏂版帴鍙楀苟瀵归綈搴忓垪鍙枫 + if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq && + notification.Seq > _multiVehicleAppliedSeq - 100) + return JsonConvert.SerializeObject(new { code = 200, message = "stale" }); + _multiVehicleAppliedSeq = notification.Seq; + MultiVehicleAligned = notification.Aligned; - lock (MultiVehicleFleet) + lock (FleetLock) + { MultiVehicleFleet = notification.Fleet ?? new Dictionary(); + // C: notify 鍐呭惈鏁撮槦鎴愬憳锛岄愪竴鍒锋柊鍏舵湰鍦板瓨娲绘椂鍒汇 + var nowSeen = DateTime.Now; + foreach (var key in MultiVehicleFleet.Keys) + _multiVehicleFleetSeen[key] = nowSeen; + } lock (_multiVehicleNotificationLock) { MultiVehicleNotification = notification; @@ -298,7 +344,14 @@ public class PilotDefinition : MultiWheelPilotDefinition 90f) { crabAngle -= 180f; speed = -speed; } - else if (crabAngle < -90f) { crabAngle += 180f; speed = -speed; } + crabInputVx = vx; + crabInputVy = vy; + crabRawAngle = crabAngle; + crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg))); + + if (crabAngle > crabSteerLimit) { crabAngle -= 180f; speed = -speed; crabReverseEquivalent = true; } + else if (crabAngle < -crabSteerLimit) { crabAngle += 180f; speed = -speed; crabReverseEquivalent = true; } fleetVx = speed; fleetFrontTh = crabAngle; fleetRearTh = crabAngle; @@ -410,8 +487,8 @@ public class PilotDefinition : MultiWheelPilotDefinition 0 + ? Conf.MultiVehicleAutoCmdTimeoutMs + : Math.Max(200, Conf.MultiVehicleSyncInterval * 4); + var autoFresh = (DateTime.Now - MultiVehicleAutoCmdTime).TotalMilliseconds < autoTimeoutMs; + if (autoFresh) + { + fleetVx = MultiVehicleAutoVx; + fleetFrontTh = MultiVehicleAutoFrontTh; + fleetRearTh = MultiVehicleAutoRearTh; + } + else + { + fleetVx = fleetFrontTh = fleetRearTh = 0; + MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0; + MultiVehicleAutoHasIdeal = false; + MultiVehicleAutoEnabled = false; + Hedingben.ToastText("鑷姩閫熷害鍛戒护瓒呮椂锛屽凡鍋滆溅锛堣矾寰勭粨鏉/鎺у埗鍣ㄥ仠鍙戯級", "MultiVehicle-auto-timeout"); + } } } else if (MultiVehicleNotification != null) @@ -438,32 +532,42 @@ public class PilotDefinition : MultiWheelPilotDefinition v.Aligned); @@ -509,9 +613,24 @@ public class PilotDefinition : MultiWheelPilotDefinition kv.Key != CarNum).All(kv => kv.Value.DetectOk); var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk); + + // H: 鑷姩妯″紡锛堥潪鎵嬪姩锛夊繀椤绘湁鏈夋晥杞﹂槦涓績鈥斺斾富杞︾敱 SLAM 鍙嶆帹銆佷粠杞︿緷璧栦富杞﹀箍鎾 fleetPosValid銆 + // 鑷姩妯″紡鏁撮槦濮挎佸缁堜緷璧 Detour锛堜笌 MultiVehicleSyncUseDetour 鏃犲叧锛夛細瀹氫綅鍏ㄧ▼涓㈠け鏃跺己鍒跺仠杞︺ + // 锛堢敤 Detour 鏃惰嫢瀹氫綅涓㈠け锛実etCartLocation() 宸插厛琛岄樆濉烇紝姝ゅ鍐嶅厹搴曡姹傛湁鏁堣溅闃熶腑蹇冦傦級 + // 鎵嬪姩妯″紡涓嶅彈闄愶紙鍏佽浠呴潬浜掕瘑鍒/閬ユ帶琛岄┒锛夈 + if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter) + { + var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _)); + if (!fleetCenterValid) + { + canMove = false; + Hedingben.ToastText("鑷姩妯″紡鏃犳湁鏁堣溅闃熶腑蹇冿紙瀹氫綅涓㈠け锛夛紝宸插仠杞", $"MultiVehicle{CarNum}-autostop"); + } + } + if (!canMove) { fleetVx = 0; @@ -531,7 +650,7 @@ public class PilotDefinition : MultiWheelPilotDefinition0 鐢ㄩ厤缃硷紝鍚﹀垯鍙 syncDistance/2锛堜笌缂栭槦鍑犱綍涓鑷达級锛屼笉鍐嶇‖缂栫爜 510銆 + var controlRadius = Conf.MultiVehicleControlRadius > 0 + ? Conf.MultiVehicleControlRadius + : syncDistance / 2f; + chassis.ControlPointRadius = controlRadius; + // #1 绾犲亸闅忔棆杞缉鏀撅細鎶婃瘡杞籂鍋忛挸鍒版棆杞垏鍚戠殑姣斾緥锛屽噺閫熸湯娈靛垏鍚戝彉灏忔椂绾犲亸鍚屾缂╁皬锛屾潨缁濊疆鍚戜贡鎽嗐 + chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac; if (canMove && Conf.MultiVehicleUseDetect) { @@ -561,7 +686,9 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega; if (canMove && rotating) { + // #3 鎶楅ケ鍜岋細涓婁竴鎷嶈埖杞湭瀵归綈(gate=0銆佽溅娌＄湡姝ｈ浆鍔)鏃跺喕缁撶Н鍒嗭紝閬垮厤鍗℃鏃剁Н鍒嗚秺绉秺澶с + var allowInteg = chassis.LastRotateAligned; var errX = detectDx + posBiasX; // mm锛岃溅浣撶郴锛氭湰杞︾旱鍚(鍓+)搴旂Щ鍔ㄩ噺 var errY = detectDy + posBiasY; // mm锛岃溅浣撶郴锛氭湰杞︽í鍚(宸+)搴旂Щ鍔ㄩ噺 var errTh = detectDth + posBiasTh; // deg锛屾湰杞﹀簲杞 rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy, Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac, - Conf.MultiVehicleRotateCompXyMax, dt); + Conf.MultiVehicleRotateCompXyMax, dt, allowInteg); rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy, Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac, - Conf.MultiVehicleRotateCompXyMax, dt); + Conf.MultiVehicleRotateCompXyMax, dt, allowInteg); rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh, Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac, - Conf.MultiVehicleRotateCompThMax, dt); + Conf.MultiVehicleRotateCompThMax, dt, allowInteg); + + // #1 绾犲亸闅忔棆杞寚浠ょ缉鏀撅細comp 脳= |fleetOmega| / 鏈宄板笺 + // 鍔犻+鍖閫熸宄板尖増褰撳墠 鈫 绯绘暟鈮1锛堝叏鍔涚籂鍋忥紝涓嶅墛寮憋級锛涘噺閫熸褰撳墠<宄板 鈫 绯绘暟闅忚浆閫熷悓姝ヤ笅闄嶃 + // 鍏抽敭锛氭棆杞垏鍚戜篃鈭濊浆閫燂紝鏁"绾犲亸:鍒囧悜"姣斾緥鍏ㄧ▼鎭掑畾=鍖閫熸姣斾緥锛堣繙<1锛夛紝鏃㈡潨缁濇湯娈佃疆瀛愪贡鎵撴柟鍚戯紝 + // 鍙堜笉鍍忔寜鍒囧悜閽充綅閭ｆ牱鍦ㄥ寑閫熸灏卞墛寮辩籂鍋忋 + var absOmega = Math.Abs(fleetOmega); + _rotOmegaPeak = Math.Max(_rotOmegaPeak, absOmega); + if (_rotOmegaPeak > 1e-3f) + { + var compScale = Math.Min(1f, absOmega / _rotOmegaPeak); + rotCompVx *= compScale; + rotCompVy *= compScale; + rotCompOmega *= compScale; + } } else { - // 妫娴嬩涪澶辨垨鏈寚浠ゆ棆杞細娓呴浂琛ュ伩骞跺浣嶇Н鍒/璁℃椂锛屽仠姝㈡椂涓嶅啀鏈夋畫鐣欓┍鍔ㄣ + // 妫娴嬩涪澶辨垨鏈寚浠ゆ棆杞細娓呴浂琛ュ伩骞跺浣嶇Н鍒/璁℃椂/宄板硷紝鍋滄鏃朵笉鍐嶆湁娈嬬暀椹卞姩銆 _rotIntegX = _rotIntegY = _rotIntegTh = 0; _rotPiLastTime = DateTime.MinValue; + _rotOmegaPeak = 0; } _mvRotCompVx = rotCompVx; @@ -642,7 +786,7 @@ public class PilotDefinition : MultiWheelPilotDefinition comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " + @@ -690,20 +838,21 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleMasterEndpoint == "/"; + /// + /// C: 鍓旈櫎瓒呰繃 TTL 鏈埛鏂帮紙register/notify锛夌殑 fleet 鎴愬憳銆傝皟鐢ㄦ柟椤诲凡鎸佹湁 FleetLock銆 + /// 浠庤溅宕╂簝/鏂綉鍚庡叾鏉＄洰杩囨湡琚垹闄わ紝浣 fleetReady(鏁伴噺==鎬绘暟) 鍚屾椂钑村惈"鍏ㄩ儴鎴愬憳鍦ㄧ嚎涓旀柊椴"锛 + /// 涓昏溅涓嶅啀鍩轰簬杩囨湡 layout/DetectOk 缁х画 SendMotion+notify銆 + /// + private void PruneStaleFleetMembers() + { + var ttlMs = Conf.MultiVehicleMemberTtlMs > 0 + ? Conf.MultiVehicleMemberTtlMs + : Math.Max(500, Conf.MultiVehicleSyncInterval * 6); + var now = DateTime.Now; + var stale = MultiVehicleFleet.Keys + .Where(k => k != CarNum && + (!_multiVehicleFleetSeen.TryGetValue(k, out var seen) || + (now - seen).TotalMilliseconds > ttlMs)) + .ToList(); + foreach (var k in stale) + { + MultiVehicleFleet.Remove(k); + _multiVehicleFleetSeen.Remove(k); + Hedingben.ToastText($"鍓旈櫎鎺夌嚎鎴愬憳 car{k}锛坽ttlMs:F0}ms 鏈埛鏂帮級", $"MultiVehicle{CarNum}-prune"); + } + } + private bool FleetHasPosAvailable() { - lock (MultiVehicleFleet) + lock (FleetLock) return MultiVehicleFleet.Values.Any(v => v.PosAvailable); } @@ -751,7 +933,7 @@ public class PilotDefinition : MultiWheelPilotDefinition others; - lock (MultiVehicleFleet) + lock (FleetLock) others = MultiVehicleFleet .Where(kv => kv.Key != CarNum && kv.Value.PosAvailable) .Select(kv => kv.Value).ToList(); @@ -764,7 +946,7 @@ public class PilotDefinition : MultiWheelPilotDefinition positioned; - lock (MultiVehicleFleet) + lock (FleetLock) positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList(); if (positioned.Count == 0) return false; @@ -791,6 +973,56 @@ public class PilotDefinition : MultiWheelPilotDefinition + /// 鐢辨湰杞︼紙閫氬父涓轰富杞︼級褰撳墠 Detour SLAM 浣嶅Э鍙嶆帹杞﹂槦涓績浣嶅Э锛堜笘鐣岀郴锛夈 + /// 澶嶇敤 GetLayoutPose + InferFleetCenterFromCar 鐨勫悓娆 SE(2) 鍙嶆帹锛坈enter = carWorld 鈭 layout鈦宦癸級锛 + /// 淇濊瘉涓庤嚜鍔ㄦā寮忚溅闃熶腑蹇冭绠椾竴鑷淬備緵鍔ㄤ綔锛堝 FleetCrabWalk锛夊彇璺緞璧风偣鐢紱鏃犳湁鏁堝畾浣嶆椂杩斿洖 false銆 + /// + public bool TryGetFleetCenterFromSlam(out float centerX, out float centerY, out float centerTh) + { + centerX = centerY = centerTh = 0; + var carPos = DetourInterface.getCartLocation(); + var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance); + var self = new VehicleSyncInfo + { + X = (float)carPos.x, + Y = (float)carPos.y, + Th = (float)carPos.th, + LayoutX = layoutX, + LayoutY = layoutY, + LayoutTh = layoutTh + }; + var (cx, cy, cth) = InferFleetCenterFromCar(self); + centerX = cx; + centerY = cy; + centerTh = cth; + return true; + } + + /// + /// 棰勭儹锛氱敤涓昏溅 SLAM 浣嶅Э锛岀珛鍗(1) 瀵瑰鍙戝竷鏈夋晥"杞﹂槦涓績蹇収"锛(2) 鎶婁富杞﹁嚜韬互 posAvailable=true + /// 鍐欏叆 fleet 琛ㄣ備緵 FleetCrabWalk 绛夊姩浣滃湪鍚姩鎺у埗鍣 Track() 鍓嶈皟鐢紝瑙ｅ喅涓や釜鍚姩鏈熼棶棰橈細 + /// - 鎺у埗鍣ㄩ甯ч氳繃 MultiVehicleGetFleetPos 璇诲埌 (0,0,0) 鈫 璇垽宸插埌缁堢偣銆佺珛鍗崇粨鏉燂紱 + /// - 鑷姩鑱斿姩寰幆鐨勫畨鍏ㄩ棬 FleetHasPosAvailable锛圥ilotDefinition.cs ~402锛夊湪鍐峰惎鍔(鏃犺溅涓婃姤瀹氫綅)鏃 + /// 浼氭妸 AutoEnabled 鍏虫帀銆佸惊鐜棤娉曞彂甯冪湡瀹炰腑蹇冦傛挱绉嶄富杞 posAvailable 鏉＄洰鍗冲彲瓒婅繃璇ラ棬锛屼娇寰幆姝ｅ父鎺ョ銆 + /// 闇鍦ㄤ富杞︿笂璋冪敤锛沢etCartLocation() 鏃犲畾浣嶆椂浼氶樆濉烇紙涓庤嚜鍔ㄦā寮忎竴鑷达級銆 + /// + public bool PrimeMasterAutoFromSlam() + { + var carPos = DetourInterface.getCartLocation(); + var (lx, ly, lth) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance); + var info = BuildSelfInfo(true, true, (float)carPos.x, (float)carPos.y, (float)carPos.th, + lx, ly, lth, false); + var (cx, cy, cth) = InferFleetCenterFromCar(info); + PublishFleetCenter(cx, cy, cth); + lock (FleetLock) + { + MultiVehicleFleet[CarNum] = info; + _multiVehicleFleetSeen[CarNum] = DateTime.Now; + } + return true; + } + private (float, float, float) GetLayoutPose(float syncTh, float syncDistance) { // 鍙岃溅缂栭槦甯冨眬鐢 TestCarSyncDistance(=syncDistance) 涓 TestCarSyncTh(=syncTh) 鍞竴纭畾锛 @@ -829,7 +1061,7 @@ public class PilotDefinition : MultiWheelPilotDefinition fleet; - lock (MultiVehicleFleet) + lock (FleetLock) fleet = new Dictionary(MultiVehicleFleet); var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh); @@ -874,10 +1106,13 @@ public class PilotDefinition : MultiWheelPilotDefinition + var url = $"http://{ip}:{port}/multi-vehicle-notify"; + var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json"); + _ = hc.PostAsync(url, content).ContinueWith(t => { + content.Dispose(); if (t.IsFaulted) DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle"); }, TaskScheduler.Default); @@ -904,18 +1139,20 @@ public class PilotDefinition : MultiWheelPilotDefinition private static float RotatePiTerm(float err, ref float integ, float deadband, - float pFac, float iFac, float max, float dt) + float pFac, float iFac, float max, float dt, bool allowIntegrate = true) { // 姝诲尯鍐咃細涓嶅啀绱姞璇樊锛屼粎杈撳嚭宸茬Н绱殑绉垎椤(缁存寔瀵规亽瀹氭壈鍔ㄧ殑绋虫佽ˉ鍋)銆 if (Math.Abs(err) < deadband) return ClampBias(integ * iFac, max); - // 鏉′欢绉垎(鎶 windup)锛氫粎褰撴昏緭鍑烘湭鍦ㄥ悓鍚戦ケ鍜屾椂鎵嶇疮鍔犺宸 - // 璧锋闃舵澶ц宸細璁 P 椤规帴杩/瓒呰繃 max锛屾鏃剁户缁Н鍒嗕細椤舵弧绉垎鍣紝 - // 璇樊鍙嶅悜鍚庨渶寰堜箙鎵嶈兘娉勬斁锛岄犳垚绾犲亸"杩囧啿鍐嶅洖鎷"銆傛澶勯ケ鍜屽嵆鍋滅Н鍒嗐 + // 鏉′欢绉垎(鎶 windup)锛氫粎褰 (a) 鍏佽绉垎(鑸佃疆宸插榻愩佽溅鍦ㄧ湡姝ｈ浆鍔) 涓 + // (b) 鎬昏緭鍑烘湭鍦ㄥ悓鍚戦ケ鍜 鏃舵墠绱姞璇樊銆 + // allowIntegrate=false锛氫笂涓鎷嶈埖杞湭瀵归綈(gate=0銆佽溅娌″姩)锛屾鏃剁Н鍒嗚宸槸绾 windup锛 + // 浼氳绾犲亸瓒婄Н瓒婂ぇ銆佽埖杞洿瀵逛笉榻 鈫 鍘熷湴鍗℃锛涙晠鍐荤粨绉垎(淇濈暀宸叉湁鍊硷紝浠呰緭鍑 P+宸茬Н鍒)銆 + // 鍚屽悜楗卞拰鍋滅Н鍒嗭細璧锋澶ц宸 P 椤舵弧 max 鏃剁户缁Н鍒嗕細椤舵弧绉垎鍣紝璇樊鍙嶅悜鍚庢硠鏀炬參銆侀犳垚杩囧啿鍥炴媺銆 var unclamped = err * pFac + integ * iFac; var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err); - if (!saturatedSameSign) + if (allowIntegrate && !saturatedSameSign) integ += err * dt; var iTerm = integ * iFac; diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs index 55369db..c6012a4 100644 --- a/MultiWheel/MultiWheelC/VehicleSyncModels.cs +++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs @@ -41,4 +41,12 @@ public class VehicleSyncNotification [JsonProperty("SyncTh")] public float SyncTh { get; set; } [JsonProperty("SyncDistance")] public float SyncDistance { get; set; } [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; } + // F: 鍗曡皟閫掑搴忓垪鍙凤紝浠庤溅鎹涓㈠純涔卞簭鍒拌揪鐨勬棫 notify 鍖呫 + [JsonProperty("Seq")] public long Seq { get; set; } + // D: 鑷姩妯″紡涓嬩富杞﹁矾寰勬帶鍒跺櫒绠楀嚭鐨勮溅闃熶腑蹇冪悊鎯充綅濮匡紙涓栫晫绯伙級锛岀敱 idealPos/idealAngle 閫忎紶鑰屾潵銆 + // HasIdeal=true 鏃跺悇浠庤溅鎸夊悇鑷 layout 鎺ㄧ畻 per-car 鐩爣浣嶅Э鍋氬墠棣+琛ュ伩锛屽姬绾胯矾寰勪笉鍐嶅彧闈犱簨鍚庣籂鍋忋 + [JsonProperty("HasIdeal")] public bool HasIdeal { get; set; } + [JsonProperty("IdealX")] public float IdealX { get; set; } + [JsonProperty("IdealY")] public float IdealY { get; set; } + [JsonProperty("IdealTh")] public float IdealTh { get; set; } } diff --git a/MultiWheel/MultiWheelM/MotorRoutine.cs b/MultiWheel/MultiWheelM/MotorRoutine.cs index de5d6a8..4ffd363 100644 --- a/MultiWheel/MultiWheelM/MotorRoutine.cs +++ b/MultiWheel/MultiWheelM/MotorRoutine.cs @@ -6,13 +6,6 @@ public class MotorRoutine : LadderLogic { public override void Operation(int iteration) { - cart.ActualSpeedLeftFront = cart.SpeedLeftFront; - cart.ActualSpeedLeftRear = cart.SpeedLeftRear; - cart.ActualSpeedRightFront = cart.SpeedRightFront; - cart.ActualSpeedRightRear = cart.SpeedRightRear; - cart.ActualThLeftFront = cart.ThLeftFront; - cart.ActualThLeftRear = cart.ThLeftRear; - cart.ActualThRightFront = cart.ThRightFront; - cart.ActualThRightRear = cart.ThRightRear; + } } diff --git a/deploy/clumsy_agv1/clumsy.json b/deploy/clumsy_agv1/clumsy.json index ad933e7..1a3b24d 100644 --- a/deploy/clumsy_agv1/clumsy.json +++ b/deploy/clumsy_agv1/clumsy.json @@ -13,7 +13,7 @@ "ManualCarSyncVxFac": 1.0, "ManualCarSyncVthFac": 1.0, "SyncThAccPerSec": 30, - "PosAvailable": true, + "MultiVehicleSyncUseDetour": true, "SimpleIp": "127.0.0.1", "CarNum": 1 }, diff --git a/deploy/clumsy_agv2/clumsy.json b/deploy/clumsy_agv2/clumsy.json index 33de771..31059a3 100644 --- a/deploy/clumsy_agv2/clumsy.json +++ b/deploy/clumsy_agv2/clumsy.json @@ -13,7 +13,7 @@ "ManualCarSyncVxFac": 1.0, "ManualCarSyncVthFac": 1.0, "SyncThAccPerSec": 30, - "PosAvailable": true, + "MultiVehicleSyncUseDetour": true, "SimpleIp": "127.0.0.1", "CarNum": 2 }, diff --git a/docs/FleetCrabWalkWorkContext.md b/docs/FleetCrabWalkWorkContext.md new file mode 100644 index 0000000..7bd7bc1 --- /dev/null +++ b/docs/FleetCrabWalkWorkContext.md @@ -0,0 +1,333 @@ +# 鑷姩锜硅锛團leetCrabWalk锛夊伐浣滀笂涓嬫枃 + +> 鏈枃妗ｆ眹鎬 **褰撳墠浠撳簱鐘舵併佸閮ㄤ緷璧栥佽繍琛/鏃ュ織璺緞銆佷唬鐮佸湴鍥句笌寰呰В鍐抽棶棰**锛屼究浜庡悗缁户缁皟璇曘岃溅闃熻仈鍔-鑷姩锜硅銆嶃 +> +> 鏈鍚庢洿鏂帮細2026-06-29 + +--- + +## 1. 鍔熻兘鐜扮姸 + +| 闃舵 | 鐘舵 | 璇存槑 | +|------|------|------| +| 鍔ㄤ綔鑳藉惎鍔ㄣ佽兘涓嬪彂杩愬姩 | 鉁 宸茶В鍐 | 鏂规 1锛堥鐑級淇浜嗗惎鍔ㄦ湡 `(0,0,0)` 蹇収瀵艰嚧 `Track()` 绔嬪嵆缁撴潫锛坄iter=0`锛夌殑闂 | +| 璺緞璺熻釜璐ㄩ噺 | 馃敡 宸叉敼锛屽緟瀹炴祴 | 2026-06-29 缁х画澶勭悊锛氳嚜鍔ㄨ煿琛屾敼涓哄鐢ㄦ墜鍔ㄨ煿琛屽悓娆 `mode=1` 涓嬪彂閾捐矾锛屽彧鍙犲姞灏忓箙骞虫粦妯悜绾犲亸 | +| 涓庢墜鍔ㄨ煿琛屽鐓 | 鉁 宸查獙璇 | 鎵嬪姩妯″紡锛團leetRemote `mode==1`锛変笣婊戯紱鍥犳鑷姩鎶栧姩涓昏鏉ヨ嚜绾犲亸閾捐矾鑰岄潪搴曠洏鎵ц鑳藉姏 | + +**瑙﹀彂鏂瑰紡**锛氫富杞 Clumsy 鈫 MovementTest 闈㈡澘 鈫 **銆岃溅闃熻仈鍔-鑷姩锜硅銆**锛坄FleetCrabWalkTest`锛夈 + +**鍓嶆彁**锛 +- 涓昏溅 `MultiVehicleMasterEndpoint="/"` +- 涓昏溅鏈 Detour 瀹氫綅锛堝弽鎺ㄨ溅闃熶腑蹇冭捣鐐癸級 +- 鍙岃溅 Medulla + Clumsy + Detour 鍧囧凡鍚姩锛岀紪闃熸垚鍛樻暟 = `MultiVehicleFleetNum` + +--- + +## 2. 褰撳墠宸茬煡闂锛堝緟鎺掓煡锛 + +### 2.1 鍚 Y+ 鏂瑰悜婕傜Щ銆佸亸绂昏矾寰 + +**鐜拌薄**锛氳溅闃熸暣浣撴部涓栫晫鍧愭爣 Y 姝ｆ柟鍚戞寔缁亸绉伙紝妯悜璇樊瓒婃潵瓒婂ぇ锛屾湭鏀舵暃鍒 `LineTrack`銆 + +**鍙兘鐩稿叧鏈哄埗**锛堟寜浼樺厛绾э紝渚涗笅涓杞鐓ф棩蹇楋級锛 + +1. **鎺у埗鍣ㄨ鍒扮殑銆屽綋鍓嶄綅濮裤嶄笌鐪熷疄 SLAM 涓績涓嶅悓姝** + - `AbstractGeometricController.Track()` 鍦 `MultiVehicleSync=true` 鏃堕氳繃 `MultiVehicleGetFleetPos()` 璇 `PilotDefinition.GetFleetCenterSnapshot()`銆 + - 宸插鐞嗭細`TickMultiVehicle` 姣忔媿寮澶寸殑 `PublishFleetCenter(0,0,0)` 宸茬Щ闄わ紝閬垮厤鍔ㄤ綔/鎺у埗绾跨▼骞跺彂璇诲埌鍋囦腑蹇冦 + +2. **妯悜绾犲亸 `bias` 椤瑰湪锜硅妯″紡涓嬬殑鍙傝冪郴** + - `MultiWheelGeometricController.PerformGoing` 涓 `bias = -bias` 鍚庢寜 Stanley 褰㈠紡淇 gcp锛坄BiasFac` / `BiasThreshold`锛夈 + - 锜硅鏃 `thDiff` 鏉ヨ嚜璺緞鍒囩嚎锛堚増澶硅锛夛紝`dTh` 鍙傝冨浐瀹 `CrabTargetHeading`锛涜嫢 `bias` 绗﹀彿鎴 fleet 涓績鏇存柊婊炲悗锛屼細鎸佺画鍚戜竴渚ф帹銆 + - 宸茬粫寮锛歚FleetCrabWalk` 褰撳墠涓嶅啀鐢ㄥ嚑浣曟帶鍒跺櫒鐩存帴涓嬪彂 gcp锛涙敼涓 Detour 璁＄畻 `along/lateral/remain`锛屽啀鍐欒剼鏈 `MultiVehicleScriptVx/Vy`銆 + +3. **`MultiVehicleSyncUseDetour=true` 鏃剁殑 POS 琛ュ伩涓庢帶鍒跺櫒鎶㈡柟鍚戠洏** + - 褰撳墠 `deploy/clumsy_agv1/clumsy.json` 涓 `MultiVehicleSyncUseDetour: true`銆 + - 鍚勮溅 SLAM 鍋忓樊缁 `PosBias*` 鍙犲姞鍒 `SendMotion`锛屽彲鑳戒笌鍑犱綍鎺у埗鍣ㄦí鍚戠籂鍋忓舰鎴愯﹀悎鎸崱銆 + +4. **鍔ㄤ綔鏈熼棿鍏抽棴浜 `MultiVehicleAutoUseIdealCenter`** + - 鏈夋剰涓轰箣锛堥伩鍏 ideal 涓績鍥炵亴蹇収銆佹姽骞崇湡瀹 bias锛夈傚壇浣滅敤鏄粎渚濊禆銆屽揩鐓т腑蹇 + bias 闂幆銆嶏紝瀵瑰揩鐓ц川閲忔洿鏁忔劅銆 + +5. **璺緞/璧风偣鍑犱綍** + - 璧风偣锛歚TryGetFleetCenterFromSlam()`锛涜矾寰勶細`LineTrack(x0,y0 鈫 dst)`锛宍phi = theta + CrabAngleDeg`銆 + - 鑻 `theta` 涓庤繍琛屾椂 `CenterTh` 涓嶄竴鑷达紝鎴 layout 鍙嶆帹涓績涓庢帶鍒跺櫒浣跨敤鐨勫揩鐓т腑蹇冩湁绯荤粺鍋忓樊锛屼細琛ㄧ幇涓烘部鏌愪竴杞存紓绉汇 + +**寤鸿涓嬩竴杞棩蹇楀鐓**锛 +- `FleetCrabDbg`锛歚lateral` 鏄惁鏀舵暃銆乣corr/localAngle/cmd` 鏄惁骞虫粦銆佹湁鏃犲埌杈剧籂鍋忎笂闄 +- `MultiVehicleDbg`锛歚frontTh/rearTh/speed` 鏄惁鎺ヨ繎鎵嬪姩锜硅銆丳OS/Detect 琛ュ伩鏄惁鍦ㄦ寔缁┍鍔紝`CRAB in/raw/limit/rev` 鏄惁鏄剧ず `raw=-95掳` 杩欑被瑙掑害鏈鍙嶅悜绛変环杞崲 +- 濡傞渶鍥為鏃у嚑浣曟帶鍒跺櫒璺嚎锛屽啀鐪 `CrabDbg` 鐨 `bias/biasItem/gcp/fleetPos` + +**2026-06-29 DLog 缁撹锛堣嚜鍔ㄨ煿琛屼粛鎶栧姩锛**锛 +- `FleetCrabDbg` 涓 `along/lateral/remain/corr/localAngle/cmd` 鍩烘湰骞虫粦锛屾í鍚戣宸鍦ㄥ嚑鍗 mm 鍐咃紝鏈璺緞鎺у埗鍣ㄥ彂鏁ｃ +- 涓/浠 `MultiVehicleDbg` 涓 `BASE vx` 鍦ㄦ璐熶箣闂磋烦锛屽悓鏃 `fTh/rTh` 鍦 `+90掳/-90掳` 闄勮繎缈昏浆锛涜繖鏄悓涓妯Щ鐭㈤噺琚敊璇湴鎸 卤90掳 杈圭晫杞崲鎴愪袱绉嶇瓑浠疯〃绀猴紝搴曠洏鎵ц灞備細鐪嬪埌鎺ヨ繎 180掳 鐨勮浆鍚戣烦鍙樸 +- POS 琛ュ伩鍦ㄨ鎵规棩蹇椾腑涓哄叧闂/闆惰ˉ鍋匡紙`corr:false`銆乣POS comp 0`锛夛紝Detect 琛ュ伩鏈夊皬骞呭间絾涓嶆槸涓诲洜銆 +- 鍥犳鏈疆鍒ゅ畾涓 **mode=1 锜硅鐭㈤噺鍚堟垚鎶 卤90掳 璇綋鑸佃杈圭晫**锛屼笉鏄紭鍏堣皟 `FleetCrabCorrectionGain`銆侻edulla 渚 `WheelAngleLowerLimit/UpperLimit` 榛樿绾︿负 `-120/+120`锛岃嚜鍔ㄨ煿琛屽簲鍏佽 `-95掳` 鐩存帴涓嬪彂銆 + +**2026-06-29 DLog 缁撹锛埪120 淇鍚庝粛 Y+ 婕傜Щ锛**锛 +- Clumsy 渚 `MultiVehicleDbg` 宸叉樉绀 `CRAB raw=-9x`銆乣limit=120.0`銆乣rev:false`锛宍BASE vx` 涓嶅啀姝ｈ礋缈昏浆锛岃鏄庝笂灞 `mode=1` 琛ㄨ揪宸茶繛缁紝鍓х儓鎶栧姩闂宸叉秷澶便 +- 浣 `FleetCrabDbg` 涓 `lateral` 浠嶄粠 `0` 鍗曡皟澧為暱鍒扮害 `+171mm`锛宍corr` 鍒拌揪 `-8掳` 涓婇檺鍚庢棤娉曟媺鍥烇紱涓/浠 `DETECT dy` 涔熷闀垮埌鐧炬绫抽噺绾э紝`DETECT comp y` 杈惧埌 `20mm/s` 涓婇檺銆 +- 杩涗竴姝ユ鏌 Playground 鍙戠幇锛歚D:\MDCS\Source\Core\Medulla\Playground\default_scene.json` 涓庤繍琛岀洰褰 `bin\Debug\net8.0\default_scene.json` 涓袱鍙 `multi-steering` 浠嶄负 `"maxSteeringAngle": 90`锛岃 `ActuatorModels.cs` 浼氭妸妯″潡鑸佃 clamp 鍒 `[-MaxSteeringAngleRad,+MaxSteeringAngleRad]`銆 +- 杩欐剰鍛崇潃 Clumsy 鍙戝嚭鐨 `-98掳` 璺緞绾犲亸锛屽湪 Playground 瀹為檯鎵ц鏃朵細琚す鍥 `-90掳`锛岀籂鍋忓垎閲忚鍚炴帀锛涜繖姣旂户缁皟 `FleetCrabCorrectionGain` 鏇村儚 Y+ 婕傜Щ鐨勭洿鎺ュ師鍥犮 +- 宸叉妸 Playground 婧愮爜鍦烘櫙鍜岃繍琛岀洰褰曞満鏅敼涓 `maxSteeringAngle: 120`锛屽苟鍦ㄤ豢鐪熷櫒涓姞鍏 `multi-steering clamp` 鑺傛祦鏃ュ織锛涘娴嬪墠蹇呴』閲嶅惎 Playground 浣垮満鏅噸杞姐傝嫢澶嶆祴鏃朵粛鍑虹幇璇ユ棩蹇楋紝璇存槑杩樻湁鍏朵粬閰嶇疆鎴栧満鏅壇鏈湪闄愬埗鑸佃銆 + +### 2.2 涓よ溅鎶栧姩銆佷笉涓濇粦 + +**鍙兘鍘熷洜**锛 + +1. 涓婅妭 **蹇収 `(0,0,0)` 绐楀彛** + 50ms 鑱斿姩鍛ㄦ湡 + 50ms `DriveTaskInterval` beat frequency +2. **notify 缁 GET fire-and-forget**锛坄MultiVehicleAutoSyncReview.md` 搂F锛夛紝浠庤溅鍛戒护闃惰穬 +3. **`dTh` 宸姩 + `bias` 闄愬箙** 鍦ㄩ槇鍊艰竟鐣屾潵鍥炲垏鎹紙`DthLinearThreshold` / `BiasThreshold`锛 +4. **`MultiVehicleSyncUseDetour` POS 琛ュ伩** 涓庝富杞︽帶鍒跺櫒涓嶅悓鐩镐綅 +5. 棰勭儹缁撴潫鍚 **`PrimeMasterAutoFromSlam` 涓嶅啀璋冪敤**锛堟甯革級锛涜嫢 `WARMUP` 鏈熼棿鏃ュ織鏄剧ず `cnt` 鍙嶅鍙樺寲锛岃鏄庣紪闃 TTL/register 涓嶇ǔ瀹 + +**寤鸿瀵圭収瀹為獙**锛 +- 鎵嬪姩 FleetRemote 锜硅锛堝悓閫熷害銆佸悓瑙掑害锛夋槸鍚︿篃鎶 +- 涓存椂 `MultiVehicleSyncUseDetour=false` 澶嶆祴 +- 鐪 `FleetCrabDbg` 鐨 `corr/localAngle/cmd` 涓 `MultiVehicleDbg` 鐨 `frontTh/rearTh` 鏄惁鍛ㄦ湡璺冲彉 + +--- + +## 3. Tutorial 浠撳簱锛堟湰浠撳簱锛 + +**璺緞**锛歚D:\MDCS\Source\Tutorial` +**鍒嗘敮**锛歚master`锛堟埅鑷虫枃妗ｇ紪鍐欐椂锛岃嚜鍔ㄨ煿琛岀浉鍏虫敼鍔**灏氭湭鍗曠嫭 commit**锛屽潎涓哄伐浣滃尯淇敼锛 + +### 3.1 宸蹭慨鏀规枃浠讹紙git status锛 + +| 璺緞 | 浣滅敤 | +|------|------| +| `MultiWheel/MultiWheelC/MovementTests.cs` | `FleetCrabWalk` / `FleetCrabWalkTest`锛涢鐑 WARMUP锛涜瘖鏂 `FleetCrabDbg` | +| `MultiWheel/MultiWheelC/PilotDefinition.cs` | `TryGetFleetCenterFromSlam`銆乣PrimeMasterAutoFromSlam`锛涜仈鍔ㄥ惊鐜紱fleet 蹇収 | +| `MultiWheel/MultiWheelC/PilotConfig.cs` | `FleetCrab*` 閰嶇疆瀛楁 | +| `MultiWheel/MultiWheelC/ChassisController.cs` | `MultiVehicleSendMotion` / `MultiVehicleGetFleetPos`锛沗SENDMOTION` 璇婃柇 | +| `MultiWheel/MultiWheelC/VehicleSyncModels.cs` | 鍚屾妯″瀷锛堣仈鍔ㄦ満鍒剁浉鍏筹級 | +| `MultiWheel/MultiWheelM/MotorRoutine.cs` | Medulla 渚х數鏈轰緥绋 | +| `deploy/clumsy_agv1/clumsy.json` | 涓昏溅 Clumsy 閰嶇疆妯℃澘 | +| `deploy/clumsy_agv2/clumsy.json` | 浠庤溅 Clumsy 閰嶇疆妯℃澘 | +| `docs/MultiVehicleConfig.md` | 搂6 鑷姩锜硅鍙傛暟璇存槑 | +| `docs/MultiVehicleAutoSyncReview.md` | 鑷姩鑱斿姩鏈哄埗闂娓呭崟 | +| `docs/RunAndDeploy.md` | 杩愯閮ㄧ讲璇存槑 | + +### 3.2 鐩稿叧鏂囨。锛堟湰浠撳簱锛 + +| 鏂囨。 | 鍐呭 | +|------|------| +| [RunAndDeploy.md](./RunAndDeploy.md) | 缂栬瘧銆佸弻杞﹀惎鍔ㄣ佺鍙/tag 瀵圭収 | +| [MultiVehicleConfig.md](./MultiVehicleConfig.md) | 鍏ㄩ儴鑱斿姩鍙傛暟锛浡6 鑷姩锜硅 | +| [MultiVehicleSync.md](./MultiVehicleSync.md) | 鑱斿姩绠楁硶鑳屾櫙 | +| [MultiVehicleAutoSyncReview.md](./MultiVehicleAutoSyncReview.md) | 鑷姩鑱斿姩宸茬煡缂洪櫡锛圓鈥揌锛 | +| [BugFixes.md](./BugFixes.md) | 鍘嗗彶淇娓呭崟 | + +--- + +## 4. 澶栭儴浠撳簱 / 渚濊禆锛堥潪 Tutorial git 绠＄悊锛 + +Tutorial 鎻掍欢閫氳繃 **`D:\MDCS\Release\`** 寮曠敤棰勭紪璇戜簩杩涘埗锛涙敼 MDCSToolbox **婧愮爜鍚庨』鍏堢紪璇戝啀缂 Tutorial**銆 + +| 缁勪欢 | 婧愮爜 / 浜х墿璺緞 | 璇存槑 | +|------|-----------------|------| +| **MDCSToolBox** | 婧愮爜锛歚D:\MDCS\Source\Products\mdcstoolbox\` | 鍑犱綍鎺у埗鍣ㄣ丅asicGo銆丩ineTrack | +| | 缂栬瘧锛歚dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release` | PostBuild 鈫 `D:\MDCS\Release\MDCSToolBox.dll` | +| | 锜硅鐩稿叧鏀瑰姩锛歚Clumsy/MotionControllers/MultiWheelGeometricController.cs` | `CrabHoldHeading` / `CrabTargetHeading`锛沗CrabDbg` | +| | | `Clumsy/MotionControllers/AbstractGeometricController.cs` | `MultiVehicleSync` 鏃惰烦杩 `firstTurnN`锛圱ODO锛 | +| | 鍙傝冿細`Clumsy/AgvInterfaces/BasicInterface.cs` | `BasicGo` + `AddTrack` 妯″紡 | +| **Clumsy** | `D:\MDCS\Release\Clumsy\ClumsyLite.exe` | 杩愯鏃跺涓 | +| **Medulla** | `D:\MDCS\Release\Medulla\` | 杞︿綋鎻掍欢瀹夸富 | +| **CommonUsage** | `D:\MDCS\Release\CommonUsage.dll` | `CommonMath`銆佸潗鏍囧彉鎹 | +| **FundamentalLib** | `D:\MDCS\Release\deps\RefFundamentalLib.dll` | `DLog` 钀界洏 | +| **Simple**锛堝彲閫夎皟搴︼級 | `D:\MDCS\Source\Core\Simple\` | `MultiWheelS` 鈫 `SimpleComposer.exe` | +| **Detour / Playground** | 閫氬父闅忎豢鐪熺幆澧冮儴缃 | 闈 Tutorial 瀛愮洰褰曪紱瑙 搂5 杩愯鐩綍 | + +### 4.1 缂栬瘧椤哄簭锛堟敼鍔ㄤ簡 MDCSToolBox 鏃讹級 + +```powershell +# 1. 宸ュ叿绠 +dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release + +# 2. Tutorial 鎻掍欢锛圡ultiWheelC PostBuild 浼氭妸 Release 涓 DLL 澶嶅埗鍒 build/Clumsy*锛 +cd D:\MDCS\Source\Tutorial +dotnet build MultiWheel\MultiWheelC\MultiWheelC.csproj +dotnet build MultiWheel\MultiWheelM\MultiWheelM.csproj +``` + +浠呮敼 Tutorial 渚 C# 鏃讹紝鍙渶绗簩姝ャ + +--- + +## 5. 娴嬭瘯鎵ц锛氱▼搴忎笌宸ヤ綔鐩綍 + +`build/` 涓 **gitignore 杩愯鐩綍**锛堥娆＄紪璇戝悗鐢熸垚锛夈備笅鍒楄矾寰勫潎鐩稿浜 `D:\MDCS\Source\Tutorial\`銆 + +### 5.1 鍙岃溅浠跨湡鍏稿瀷鍚姩椤哄簭 + +1. **Playground**锛堜豢鐪熷満鏅紝鍚 `agv_multi_1` / `agv_multi_2`锛 +2. **Detour 脳2**锛堝伐浣滅洰褰曚竴鑸湪 Clumsy build 鏍戜笅锛 +3. **Medulla 脳2** +4. **ClumsyLite 脳2** + +| 瑙掕壊 | 宸ヤ綔鐩綍 | 涓荤▼搴 | 鍏抽敭閰嶇疆 | +|------|----------|--------|----------| +| AGV1 涓昏溅 | `build\Medulla\` | Medulla 鎺у埗鍙 | `startup.iocmd`锛歚SetShareObjectTag Multi1`锛宍CarNum 1` | +| AGV1 Clumsy | `build\Clumsy\` | `ClumsyLite.exe` | `deploy\clumsy_agv1\` 妯℃澘锛沺ort **8008** | +| AGV1 Detour | `build\Clumsy\`锛堟垨鍚屾爲 `Detour\`锛 | DetourLite | HTTP **4321**锛宼ag `Multi1` | +| AGV2 浠庤溅 | `build\Medulla_AGV2\` | Medulla 鎺у埗鍙 | tag `Multi2`锛宍CarNum 2` | +| AGV2 Clumsy | `build\Clumsy_AGV2\` | `ClumsyLite.exe` | port **8009**锛宮aster `127.0.0.1:8008` | +| AGV2 Detour | `build\Clumsy_AGV2\Detour_AGV2\` 绛 | DetourLite | HTTP **4421**锛宼ag `Multi2` | + +**蹇嵎鑴氭湰**锛堝湪宸查厤缃ソ鐨 build 鐩綍鍐咃級锛 +- `deploy\start_clumsy_agv1.bat` 鈫 澶嶅埗閰嶇疆鍚庡惎鍔 `ClumsyLite.exe`锛堜富杞︼級 +- `deploy\start_clumsy_agv2.bat` 鈫 浠庤溅 +- 涓閿 7 杩涚▼锛堣嫢鐜宸茶锛夛細`DetourLite\bin\Debug\net8.0\start_all_sim.bat`锛堣矾寰勮 [RunAndDeploy.md](./RunAndDeploy.md)锛 + +**缂栬瘧浜х墿钀界偣**锛 + +| 椤圭洰 | 杈撳嚭 | +|------|------| +| `MultiWheelC.csproj` | `build\Clumsy\MultiWheelC.dll` + PostBuild 鍚屾鍒 `build\Clumsy_AGV2\` | +| `MultiWheelM.csproj` | `build\Medulla\plugins\MultiWheelM.dll`锛圓GV2 Medulla 闇鍙﹁澶嶅埗鎴 PostBuild锛 | + +### 5.2 瑙﹀彂鑷姩锜硅娴嬭瘯 + +1. 鎸変笂琛ㄥ惎鍔ㄥ弻杞︽爤 +2. 涓昏溅 Medulla 寮鍚岃溅闃熻仈鍔ㄣ嶏紙鎵嬪姩鑱旇皟鏃跺父鎸 **F**锛涚函鑷姩锜硅 MovementTest 渚濊禆 `MultiVehicleAutoEnabled`锛屽姩浣滃唴浼氳嚜琛岀疆浣 + 棰勭儹锛 +3. 涓昏溅 `build\Clumsy\` 鐨 Clumsy UI 鈫 MovementTest 鈫 **杞﹂槦鑱斿姩-鑷姩锜硅** + +鍙傛暟鏉ユ簮锛歚clumsy.json` 鈫 `msConf` 鈫 `PilotConfig`锛堟湭鍐欏叆 json 鐨勫瓧娈电敤浠ｇ爜榛樿鍊硷級銆 + +--- + +## 6. DLog 鏃ュ織鐩綍涓 Topic + +### 6.1 钀界洏鏍圭洰褰 + +DLog 鐢 **Clumsy 杩涚▼宸ヤ綔鐩綍**涓嬬殑 `dlog\` 绠＄悊锛團undamentalLib锛夈傚弻杞︿豢鐪熸椂锛 + +| 杩涚▼ | 鏃ュ織鏍圭洰褰 | +|------|------------| +| 涓昏溅 Clumsy | `D:\MDCS\Source\Tutorial\build\Medulla\dlog\` | +| 浠庤溅 Clumsy | `D:\MDCS\Source\Tutorial\build\Medulla_AGV2\dlog\` | + +> 璇存槑锛氱敤鎴峰疄娴嬭矾寰勪负涓婅堪涓ゅ锛泃opic 鍚嶅搴斿瓙鏂囦欢澶/鏂囦欢銆傝嫢 Clumsy 宸ヤ綔鐩綍 strictly 涓 `build\Clumsy*`锛屼篃鍙兘鍦 `build\Clumsy\dlog\` 鈥斺 **浠ュ疄闄呰繘绋 cwd 涓嬫槸鍚︾敓鎴 `dlog` 涓哄噯**銆 + +鐩綍缁撴瀯锛堟蹇典笂锛夛細`dlog\\` 涓嬫寜 topic 婊氬姩锛涘悓涓 topic 鐨 `DLog.Log(msg, topic)` 褰掑苟鍒板悓涓鐩綍銆 + +### 6.2 鑷姩锜硅鐩稿叧 Topic + +| Topic | 鏉ユ簮 | 鍐呭 | +|-------|------|------| +| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`锛屽惈 `along/lateral/remain/corr/localAngle/cmd` | +| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 鏃у嚑浣曟帶鍒跺櫒璺嚎璇婃柇锛涘綋鍓嶈剼鏈煿琛屽疄鐜颁笉鍐嶄緷璧 | +| **`MultiVehicleDbg`** | `PilotDefinition.cs` | 鑱斿姩寰幆锛氶熷害銆佽埖瑙掋佽ˉ鍋裤乺eady 鐘舵 | +| **`FleetDiagClumsy`** | `PilotDefinition.cs` | 绮剧畝 fleet 璇婃柇锛堝甫 `car{N}` 鍓嶇紑锛 | +| **`MultiVehicle`** | `PilotDefinition.cs` | 鍒濆鍖栥佸績璺炽丠TTP 閿欒 | +| **`MotionControl`** | 鎺у埗鍣ㄦ鏋 | 閫氱敤杩愬姩鎺у埗锛堣嫢鍚敤锛 | + +### 6.3 寤鸿鎶撳彇椤哄簭锛堟帓鏌ユ紓绉/鎶栧姩锛 + +1. 涓昏溅 `FleetCrabDbg`锛歚WARMUP done` 鈫 `ITER#` 涓 `lateral/remain/corr/localAngle/cmd` +2. 涓昏溅 + 浠庤溅 `MultiVehicleDbg`锛歚frontTh/rearTh`銆乣PosBias*`銆佹槸鍚 `ready=false` +3. 鑻ュ洖閫鏃у嚑浣曟帶鍒跺櫒璺嚎锛屽啀鐪嬩富杞 `CrabDbg`锛歚bias` 鏄惁鍗曡皟澧炲ぇ锛沗fleetPos` 鏄惁鍋跺彂 `(0,0,0)` +4. 浠庤溅 `FleetDiagClumsy`锛氭槸鍚﹂绻佹帀绾 / register 瓒呮椂 + +--- + +## 7. 浠ｇ爜鍦板浘锛堟暟鎹祦锛 + +```text +MovementTest銆岃溅闃熻仈鍔-鑷姩锜硅銆 + FleetCrabWalk.Get() + TryGetFleetCenterFromSlam() 鈫 璺緞璧风偣 (x0,y0,胃) + phi = theta + FleetCrabAngleDeg + MultiVehicleScriptEnabled = true + MultiVehicleScriptMode = 1 鈫 澶嶇敤 FleetRemote 鎵嬪姩锜硅涓嬪彂閾捐矾 + WARMUP 鈫 绛夌紪闃熸垚鍛樺氨浣 + loop: + TryGetFleetCenterFromSlam() 鈫 褰撳墠杞﹂槦涓績 + along/lateral/remain 鈫 娌跨嚎杩涘害銆佹í鍚戝亸宸佸墿浣欒窛绂 + corr = clamp(Stanley(lateral), 卤FleetCrabCorrectionAngleDeg) + localAngle = (phi + corr) - currentTheta + Vx/Vy slew limit 鈫 FleetCrabCommandAccel 骞虫粦 + MultiVehicleScriptVx/Vy = cmd + PilotDefinition.TickMultiVehicle (50ms) + manual/script mode==1 + Vx/Vy 鈫 speed + frontTh==rearTh + notify 鈫 浠庤溅 SendMotion + POS/Detect 琛ュ伩 +``` + +**瀵圭収 baseline**锛歚PilotDefinition.cs` 鎵嬪姩鍒嗘敮 `fleetMode == 1`锛團leetRemote 锜硅锛夌洿鎺ュ悎鎴 `frontTh/rearTh`锛屼笉缁忓嚑浣曟帶鍒跺櫒 `bias` 闂幆銆 + +--- + +## 8. 閰嶇疆鍙傛暟閫熸煡 + +### 8.1 鑷姩锜硅涓撶敤锛坄PilotConfig` / `msConf`锛 + +| 瀛楁 | 榛樿 | 浣滅敤 | +|------|------|------| +| `FleetCrabAngleDeg` | 45 | 璺緞涓庤溅闃熸湞鍚戝す瑙 (deg) | +| `FleetCrabLengthMm` | 2000 | 璺緞闀垮害 (mm) | +| `FleetCrabSpeed` | 0.2 | 閫熷害 (m/s) | +| `FleetCrabGcpThetaThreshold` | 95 | 鍏煎鏃у嚑浣曟帶鍒跺櫒瀹炵幇锛涘綋鍓嶈剼鏈煿琛屼笉鐩存帴浣跨敤 | +| `FleetCrabCorrectionGain` | 1.0 | 妯悜璇樊绾犲亸澧炵泭 | +| `FleetCrabCorrectionAngleDeg` | 8 | 鑷姩绾犲亸鏈澶ф敼鍚戣锛岃秺灏忚秺鎺ヨ繎鎵嬪姩锜硅 | +| `FleetCrabCommandAccel` | 0.4 | 鑴氭湰 `Vx/Vy` 鍛戒护鏂滅巼闄愬埗(m/s虏) | + +鍔ㄤ綔琛屼负锛氬綋鍓嶄笉鍐嶆敼 `MultiVehicleAutoUseIdealCenter`锛岀粨鏉/鎬ュ仠浼氭竻闆 `MultiVehicleScript*` 鍜 `MultiVehicleAuto*`銆 + +### 8.2 褰卞搷璺熻釜/鎵嬫劅鐨勫叏灞椤癸紙鑺傞夛級 + +| 瀛楁 | deploy 涓昏溅褰撳墠鍊 | 澶囨敞 | +|------|-------------------|------| +| `MultiVehicleSyncUseDetour` | **true** | 閫愯溅 SLAM POS 琛ュ伩锛涙鐤戜笌婕傜Щ/鎶栧姩鐩稿叧 | +| `MultiVehicleUseDetect` | false锛堥粯璁わ級 | true 鏃朵簰璇嗗埆瀹夊叏闂 | +| `TestCarSyncDistance` | 2400 | 涓 Playground 鍙岃溅闂磋窛涓鑷 | +| `MultiVehicleSyncInterval` | 50 | 鑱斿姩鍛ㄦ湡 ms | +| `DriveTaskInterval` | 50 | `clumsy.json` 椤跺眰 | +| `BiasFac` / `DthLinearFac` 绛 | 缁ф壙 `MultiWheelPilotConfig` | 鍑犱綍鎺у埗鍣 PID 褰㈡佸弬鏁 | + +璇﹁ [MultiVehicleConfig.md](./MultiVehicleConfig.md) 搂2鈥撀6銆 + +--- + +## 9. 宸插疄鐜扮殑鍏抽敭淇锛堜究浜庡洖婧級 + +| 闂 | 澶勭悊 | +|------|------| +| 鑷姩锜硅瀹屽叏涓嶅姩 (`iter=0`) | 鏂规1锛歚PrimeMasterAutoFromSlam` + WARMUP 鍚庡啀 `Track()` | +| 锜硅瑕佹眰鏈濆悜涓嶅彉浣嗘湁绾犲亸 | `CrabHoldHeading` + `CrabTargetHeading`锛涗繚鐣 `dTh` | +| 澶氳溅 firstTurn 鐮村潖闃熷舰 | `MultiVehicleSync` 鏃惰烦杩 `firstTurnN`锛圱ODO 鏁撮槦棰勬棆杞級 | +| gcp 琚 45掳 涓婇檺鎴柇 | 鍔ㄤ綔渚 `GcpThetaThreshold=95` | +| ideal 涓績鎶瑰钩妯悜璇樊 | 鍔ㄤ綔鏈熼棿鍏 `MultiVehicleAutoUseIdealCenter` | +| Tick 涓棿绐楀彛鍙戝竷 `(0,0,0)` 鍋囦腑蹇 | 宸茬Щ闄 tick 寮澶 `PublishFleetCenter(0,0,0)` | +| 鑷姩锜硅绾犲亸瀵艰嚧鎶栧姩 | 宸叉敼涓鸿剼鏈墜鍔ㄨ煿琛岄摼璺 + 灏忓箙骞虫粦妯悜绾犲亸 | +| 鎺ヨ繎绾í绉绘椂閫熷害绗﹀彿/鑸佃琛ㄧず缈昏浆 | `fleetMode==1` 鏀逛负鎸 `MultiVehicleCrabSteerLimitDeg`锛堥粯璁 120掳锛夊綊涓鍖栵紱`-95掳` 鐩存帴涓嬪彂锛岃秴杩囦笂闄愭墠鍋氶熷害鍙栧弽鐨勭瓑浠疯浆鎹紝骞跺湪 `MultiVehicleDbg` 杈撳嚭 `CRAB in/raw/limit/rev` | + +--- + +## 10. 鍚庣画宸ヤ綔寤鸿锛堜紭鍏堢骇锛 + +1. **澶嶆祴 -90掳 鑷姩锜硅**锛氶噸鐐圭湅 `MultiVehicleDbg` 鐨 `CRAB raw=-9x`銆乣limit=120.0`銆乣rev:false`锛屼互鍙 `BASE vx/fTh/rTh` 鏄惁涓嶅啀姝ｈ礋缈昏浆銆 +2. **A/B锛歚MultiVehicleCrabSteerLimitDeg`** 榛樿 120锛屽簲涓 Medulla 渚 `WheelAngleLowerLimit/UpperLimit` 鍖归厤锛涜嫢瀹為檯杞闄愬埗涓嶅悓锛屽厛鍚屾璇ュ笺 +3. **A/B锛歚FleetCrabCorrectionAngleDeg`** 鍏堣瘯 4銆8銆12锛4 鏈鎺ヨ繎鎵嬪姩锛12 鏀舵暃鏇村揩锛涘綋鍓嶄笉鍐嶅洜璺 卤90掳 鐩存帴缈婚潰銆 +4. **A/B锛歚MultiVehicleSyncUseDetour=false`** 鑻ヤ粛鎶栵紝璺戝悓涓鏉¤煿琛岋紝鍖哄垎鑴氭湰绾犲亸 vs POS 琛ュ伩璐＄尞銆 +5. **璺緞璇樊**锛氳嫢浠嶆寔缁 Y+ 婕傜Щ锛岀湅 `lateral` 鏄惁鎸佺画鍗曞悜澧為暱锛涜嫢澧為暱浣 `corr` 宸插埌涓婇檺锛屽澶 `FleetCrabCorrectionAngleDeg` 鎴 `FleetCrabCorrectionGain`銆 +6. **notify 骞虫粦**锛堜腑闀挎湡锛夛細瑙 `MultiVehicleAutoSyncReview.md` 搂F銆 + +--- + +## 11. 蹇熷懡浠ゅ蹇 + +```powershell +# 缂栬瘧 +dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release +dotnet build D:\MDCS\Source\Tutorial\MultiWheel\MultiWheelC\MultiWheelC.csproj + +# 鏌ョ湅宸ヤ綔鍖虹姸鎬 +cd D:\MDCS\Source\Tutorial +git status --short + +# 鏌ョ湅鏈鏂 FleetCrab 鏃ュ織锛堜富杞︼紝PowerShell锛 +Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\FleetCrabDbg -ErrorAction SilentlyContinue | + Sort-Object LastWriteTime -Descending | Select-Object -First 3 +Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\CrabDbg -ErrorAction SilentlyContinue | + Sort-Object LastWriteTime -Descending | Select-Object -First 3 +``` diff --git a/docs/MultiVehicleAutoSyncReview.md b/docs/MultiVehicleAutoSyncReview.md index 48f6c21..5d6e622 100644 --- a/docs/MultiVehicleAutoSyncReview.md +++ b/docs/MultiVehicleAutoSyncReview.md @@ -194,3 +194,40 @@ lock (MultiVehicleFleet) | 鏃ユ湡 | 璇存槑 | |------|------| | 2026-06-28 | 鍒濈増锛氬熀浜 Tutorial MultiWheelC 鑷姩鑱斿姩瀹炵幇涓庤仈璋冩棩蹇楀垎鏋愭暣鐞 | +| 2026-06-28 | A~H 鍏ㄩ儴淇钀藉湴锛圥ilotDefinition.cs / ChassisController.cs / VehicleSyncModels.cs / PilotConfig.cs锛夛紝瑙佷笅銆屼慨澶嶅疄鐜般 | +| 2026-06-29 | 淇 `MultiVehicleSyncUseDetour` 璇箟锛氫粎鎺у埗"杞﹂槦鍐呭Э鎬佺籂姝"锛屾暣杞﹂槦濮挎佽绠楀缁堢敤 Detour锛涘苟淇 `FleetRotateInPlace` 娆犺浆锛堣埅鍚戦棴鐜垽鍋滐級锛岃涓嬨岃涔変慨姝ｃ | + +## 淇瀹炵幇锛圓~H锛 + +- **A**锛氭柊澧 `public readonly object FleetLock`锛屾墍鏈夊 `MultiVehicleFleet` 鐨勮鍐欑粺涓 `lock(FleetLock)`锛堝惈 ChassisController 鍥炶皟锛夛紝涓嶅啀閿佷細琚暣浣撴浛鎹㈢殑瀛楁寮曠敤銆 +- **B**锛歚MultiVehicleSendMotion` 鍥炶皟鍐欏叆 `MultiVehicleAutoCmdTime`锛涗富杞﹁嚜鍔ㄥ垎鏀寜 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 鍒ゅ畾鍛戒护鏂伴矞搴︼紝瓒呮椂娓呴浂閫熷害/idealPos 骞跺叧闂 `AutoEnabled`锛岄伩鍏嶆湯閫熷害婊戣銆 +- **C**锛氭柊澧炴湰鍦 `_multiVehicleFleetSeen` 瀛樻椿鏃跺埢琛紝register/notify 鏀跺埌鍗冲埛鏂帮紱涓昏溅 Tick `PruneStaleFleetMembers()` 鎸 `MultiVehicleMemberTtlMs`(0=auto) 鍓旈櫎鎺夌嚎鎴愬憳锛宍fleetReady`(鏁伴噺==鎬绘暟) 鍥犳钑村惈鍏ㄥ憳鏂伴矞銆 +- **D**锛氬洖璋冧笉鍐嶄涪寮 `idealPos/idealAngle`锛屽啓鍏 `MultiVehicleAutoIdeal*` 骞剁粡 notify(`HasIdeal/IdealX/Y/Th`) 骞挎挱锛涜嚜鍔ㄦā寮忎笅浠ョ悊鎯宠溅闃熶腑蹇冧綔涓哄悇杞 layout 鍓嶉鐩爣锛坄MultiVehicleAutoUseIdealCenter`锛岄粯璁ゅ紑锛夈 +- **E**锛氭柊澧 `MultiVehicleControlRadius`(0=syncDistance/2)锛宍ControlPointRadius` 涓 `SendMotion(localControlRadius)` 缁熶竴鍙栬鍊硷紝鍒犻櫎纭紪鐮 510銆 +- **F**锛歯otify 鏀逛负 POST + JSON body锛堝彇浠 GET query 涓诧級锛涙柊澧炲崟璋冮掑 `Seq`锛屼粠杞︿涪寮冧贡搴忔棫鍖咃紙鍚富杞﹂噸鍚洖閫璇嗗埆锛夈 +- **G**锛氭柊澧 `FleetCenterSnapshot` 涓嶅彲鍙樺揩鐓 + `volatile` 寮曠敤锛宍PublishFleetCenter` 鏁翠綋璧嬪硷紝鎺у埗鍣ㄧ嚎绋 `GetFleetCenterSnapshot()` 鍙瀹屾暣蹇収锛屾秷闄 torn read銆 +- **H**锛氳嚜鍔ㄦā寮忔柊澧 `MultiVehicleAutoRequireFleetCenter`(榛樿寮) 闂ㄦ帶鈥斺旀棤鏈夋晥杞﹂槦涓績锛堝畾浣嶄涪澶憋級鏃跺己鍒跺仠杞︼紝琛ヤ笂绾 SLAM 妯″紡瀹夊叏缃戯紱鎵嬪姩妯″紡涓嶅彈闄愩 + +## 璇箟淇锛2026-06-29锛 + +### 1. `MultiVehicleSyncUseDetour` 閲嶆柊瀹氫箟锛氫粎鎺у埗"杞﹂槦鍐呭Э鎬佺籂姝" + +**闂**锛氬師瀹炵幇鎶"鏄惁璇 Detour 鍏ㄥ眬浣嶅Э"涓"鏄惁鍋氳溅闃熷唴濮挎佺籂姝"缁戝湪鍚屼竴寮鍏充笂銆俙false` 鏃 `posAvailable` 鐩存帴涓哄亣銆佹牴鏈笉璇 SLAM锛屽鑷 `TryInferFleetCenter` 澶辫触銆佹暣杞﹂槦濮挎佹棤娉曡绠椻斺旇繖涓庤寮鍏冲簲鏈夌殑鍚箟涓嶇銆 + +**淇**锛坄PilotDefinition.TickMultiVehicle`锛夛細灏嗗崟涓 `posAvailable` 鎷嗕负涓変釜璇箟娓呮櫚鐨勯噺锛 + +| 鍙橀噺 | 鍚箟 | 鍙栧 | +|------|------|------| +| `useDetourCorrection` | 鏄惁鍋**杞﹂槦鍐呭Э鎬佺籂姝**锛坄PosBias*` 閫愯溅 SLAM 琛ュ伩锛 | `= MultiVehicleSyncUseDetour` | +| `slamRead` | 鏈溅鏈疆鏄惁璇诲彇 Detour 鍏ㄥ眬浣嶅Э | `useDetourCorrection \|\| (isMaster && autoMode)` | +| `fleetPosValid` | 鏁磋溅闃熷叏灞濮挎佹槸鍚﹀凡鐭ワ紙涓昏溅=鑷韩璇诲埌锛屼粠杞=涓昏溅骞挎挱锛 | 瑙佷唬鐮 | + +- **鏁磋溅闃熷Э鎬佽绠**锛堝弽鎺/骞挎挱杞﹂槦涓績銆丼LAM 闂磋窛銆佽嚜鍔ㄥ畨鍏ㄩ棬 H銆佽嚜鍔ㄥ叆鍙ｉ棬锛変竴寰嬫敼鐢 `slamRead`/`fleetPosValid`锛**濮嬬粓渚濊禆 Detour**锛屼笉鍐嶅彈寮鍏抽檺鍒讹紱鑷姩鍏ュ彛闂ㄤ笌 H 闂ㄥ幓鎺 `&& MultiVehicleSyncUseDetour` 鏉′欢銆 +- **杞﹂槦鍐呭Э鎬佺籂姝**锛坄PosBias*` 琛ュ伩鍧楋級鏄敮涓鍙 `useDetourCorrection` 鎺у埗鐨勫紑鍏崇偣銆 +- 鍗 `false` 鏃讹細鑷姩妯″紡涓昏溅浠 `getCartLocation()` 璁＄畻鏁磋溅闃熷Э鎬侊紙鏃犲畾浣嶅垯闃诲鍋滆溅锛夛紝浣嗕笉鍐嶉愯溅 SLAM 绾犲亸銆 + +### 2. `FleetRotateInPlace` 娆犺浆淇锛堣埅鍚戦棴鐜垽鍋滐級 + +**闂**锛氬師鍦版棆杞 MovementDefinition 鐨勫垽鍋滄部鐢 `MultiVehicleSyncUseDetour`锛屽叧闂椂閫鍖栦负"鎸変及绠楁椂闀垮紑鐜仠姝"锛屽疄闄呰浆閫 < 鎸囦护鏃讹紙PI 绾犲亸鍚冮熺巼 + 璧锋鏂滃潯锛変細**娌¤浆鍒扮洰鏍囧氨鍋**锛堝疄娴 180掳 娆犺浆锛夈 + +**淇**锛坄MovementTests.FleetRotateInPlace`锛夛細鏂板 `FleetRotateUseDetourHeading`(榛樿 `true`)锛**涓 `MultiVehicleSyncUseDetour` 瑙ｈ**鈥斺旇浆鍒版寚瀹氳搴﹀睘浜"鏁磋溅闃熷Э鎬佽绠"锛屾晠榛樿璇讳富杞 SLAM 鑸悜闂幆绱瀹為檯杞锛屽埌 `|TargetDeltaDeg|` 鎵嶅仠銆傛柊澧 `FleetRotateDbg` 钀界洏鏃ュ織锛堝疄闄呰埅鍚/绱杞/瀹為檯vs鎸囦护瑙掗熺巼/鍒ゅ仠鍘熷洜锛変究浜庡鐜版牳瀵广 diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md index 60709a0..4d76524 100644 --- a/docs/MultiVehicleConfig.md +++ b/docs/MultiVehicleConfig.md @@ -117,20 +117,61 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) | `MultiVehicleUseDetect` | 鍚敤浜掕瘑鍒籂姝 **+ 瀹夊叏闂**锛堜换涓杞︽娴嬩笉鍒伴偦杞︹啋鏁撮槦鍋滆溅锛 | `true` | | `MultiVehicleDetectBiasXFac/YFac/ThFac` | 浜掕瘑鍒ˉ鍋跨郴鏁 | `0.5/0.5/0.5` | | `MultiVehicleDetectBiasXThreshold/YThreshold/ThThreshold` | 浜掕瘑鍒ˉ鍋夸笂闄(mm/mm/deg) | `50/50/5` | -| `MultiVehiclePosBiasXFac/YFac/ThFac` | SLAM 缂栭槦淇濇寔琛ュ伩绯绘暟 | `0.5/0.5/0.5` | -| `MultiVehiclePosBiasXThreshold/YThreshold/ThThreshold` | SLAM 琛ュ伩涓婇檺(mm/mm/deg) | `50/50/5` | +| `MultiVehiclePosBiasXFac/YFac/ThFac` | **杞﹂槦鍐呭Э鎬佺籂姝**锛圫LAM 閫愯溅缂栭槦淇濇寔锛夎ˉ鍋跨郴鏁 | `0.5/0.5/0.5` | +| `MultiVehiclePosBiasXThreshold/YThreshold/ThThreshold` | 杞﹂槦鍐呭Э鎬佺籂姝ｄ笂闄(mm/mm/deg) | `50/50/5` | | `SingleCarSyncPrecisionXy` / `SingleCarSyncPrecisionTh` | 瀵归綈绮惧害 / 琛ュ伩姝诲尯(mm/deg) | `10 / 0.2` | -| `PosAvailable` | 鏄惁鍚敤 Detour 瀹氫綅 | `true` | +| `MultiVehicleSyncUseDetour` | **浠**鎺у埗"瀹氫綅鏄惁鍙備笌**杞﹂槦鍐呭Э鎬佺籂姝**"锛堝嵆涓婇潰鐨 `PosBias*` 琛ュ伩锛夛紱**涓嶅奖鍝**"鏁翠釜杞﹂槦濮挎佺殑璁＄畻" | `false` | + +**`MultiVehicleSyncUseDetour` 璇箟锛堥噸瑕侊紝鍕挎贩娣嗭級** + +璇ュ紑鍏冲彧鍒囨崲 **"杞﹂槦鍐呭Э鎬佺籂姝"**锛堢敤 SLAM 閫愯溅鎶婃瘡鍙拌溅绾犲洖鍏剁紪闃 slot锛屽嵆 `PosBias*` 琛ュ伩锛夛紝**涓**鍒囨崲 **"鏁翠釜杞﹂槦濮挎佺殑璁＄畻"**锛 + +| 鐢ㄩ | 鏄惁鍙楄寮鍏虫帶鍒 | 璇存槑 | +|------|------------------|------| +| 杞﹂槦鍐呭Э鎬佺籂姝ｏ紙`PosBias*` 閫愯溅 SLAM 琛ュ伩锛 | **鏄**锛坒alse=鍏抽棴锛屼粎闈犵紪闃熷嚑浣/浜掕瘑鍒繚鎸侀槦褰級 | 鍞竴寮鍏崇偣 | +| 鍙嶆帹/骞挎挱杞﹂槦涓績銆丼LAM 闂磋窛銆佽嚜鍔ㄥ畨鍏ㄩ棬銆佸師鍦版棆杞垽鍋滆埅鍚 | **鍚︼紝濮嬬粓鐢 Detour** | 鑷姩妯″紡鏁撮槦濮挎佹亽渚濊禆鍏ㄥ眬瀹氫綅 | + +- 鍗充娇 `MultiVehicleSyncUseDetour=false`锛**鑷姩妯″紡涓昏溅浠嶈皟鐢 `getCartLocation()`** 鍙嶆帹杞﹂槦涓績锛涜嫢鏃犳湁鏁堝叏灞瀹氫綅鍒欒璋冪敤闃诲 鈫 鑱斿姩绾跨▼闃诲涓嶄笅鍙戦熷害锛堝畨鍏ㄥ仠杞︼級锛屽畾浣嶆仮澶嶅悗鑷姩缁х画銆 +- `false` 閫傜敤浜庯細SLAM 涓よ溅鐩稿绮惧害涓嶄匠銆佸笇鏈涘彧闈犱簰璇嗗埆/缂栭槦鍑犱綍淇濇寔闃熷舰锛屼絾鏁磋溅闃熺殑缁濆浣嶅Э浠嶇敱 Detour 椹卞姩锛堝鑷姩寰矾寰勶級銆 **瑕佺偣** - `MultiVehicleUseDetect=true` 鏃惰嫢 2 鑵挎娴嬫病閿佸畾锛宍fleetVx` 浼氳瀹夊叏闂ㄧ疆闆讹紙琛ㄧ幇涓烘憞鏉"鏃犳晥"鈥斺旇繖鏄鏈熷畨鍏ㄨ涓猴紝涓嶆槸 bug锛夈傚厛纭繚妫娴嬬ǔ瀹氥 -- SLAM 琛ュ伩锛坄PosBias*`锛夎姹備袱杞**鍏变韩鍚屼竴 SLAM 涓栫晫绯**锛涘惁鍒欑紪闃熶腑蹇冨弽鎺ㄤ細閿欍侾layground 涓よ溅鍚屽浘锛屾弧瓒炽 -- 涓嶉渶瑕佺粷瀵圭紪闃熶繚鎸佹椂锛屽彲鎶 `MultiVehiclePosBias*Fac` 璁 0锛屼粎闈犱簰璇嗗埆缁存寔闂磋窛銆 +- 杞﹂槦鍐呭Э鎬佺籂姝ｏ紙`PosBias*`锛夎姹備袱杞**鍏变韩鍚屼竴 SLAM 涓栫晫绯**锛涘惁鍒欑紪闃熶腑蹇冨弽鎺ㄤ細閿欍侾layground 涓よ溅鍚屽浘锛屾弧瓒炽 +- 涓嶉渶瑕佺粷瀵圭紪闃熶繚鎸佹椂锛屽彲鎶 `MultiVehiclePosBias*Fac` 璁 0 鎴 `MultiVehicleSyncUseDetour=false`锛屼粎闈犱簰璇嗗埆缁存寔闂磋窛锛堟暣杞﹂槦濮挎佷粛鐢 Detour 璁＄畻锛夈 --- -## 6. 甯歌鍧戜綅锛堟帓鏌ユ竻鍗曪級 +## 6. 鑷姩锜硅鍔ㄤ綔锛團leetCrabWalk / MovementTest銆岃溅闃熻仈鍔-鑷姩锜硅銆嶏級 + +鍦 Clumsy 渚 MovementTest 闈㈡澘瑙﹀彂锛屼互**褰撳墠杞﹂槦涓績**涓鸿捣鐐癸紝鏋勯犱竴鏉′笌杞﹂槦鏈濆悜澶硅 `FleetCrabAngleDeg`銆侀暱搴 `FleetCrabLengthMm` 鐨**鐩寸嚎璺緞**锛屾墽琛屼晶澶嶇敤 FleetRemote 宸查獙璇佷笣婊戠殑鑴氭湰鎵嬪姩绛変环杈撳叆锛坄MultiVehicleScriptEnabled + mode=1`锛夎鏁撮槦**鏂滃悜骞崇Щ锛堣煿琛岋級**锛 + +- 鍔ㄤ綔姣忔媿璇诲彇涓昏溅 Detour 鍙嶆帹杞﹂槦涓績锛岃绠楃洿绾胯繘搴 `along`銆佹í鍚戝亸宸 `lateral` 鍜屽墿浣欒窛绂 `remain`銆 +- 妯悜鍋忓樊鍙浆鎴愪竴涓**灏忓箙銆佸甫鏂滅巼闄愬埗鐨勮煿琛屾柟鍚戜慨姝**锛屽啀鍐欏叆 `MultiVehicleScriptVx/Vy`锛沗TickMultiVehicle` 浠嶆寜鎵嬪姩锜硅閫昏緫鍚堟垚 `frontTh==rearTh` 骞跺箍鎾粠杞︺ +- 杩欐牱淇濈暀鎵嬪姩锜硅鐨勫钩婊戞墽琛岄摼璺紝鍚屾椂璁╄嚜鍔ㄥ姩浣滃叿澶囨俯鍜岀殑璺緞绾犲亸锛涢伩鍏嶆棫鍑犱綍鎺у埗鍣 `bias/dTh` 鐩存帴鍙犲埌 gcp 鏃跺嚭鐜拌埖瑙掗樁璺冦 +- **鍓嶆彁**锛氬湪**涓昏溅**锛坄MultiVehicleMasterEndpoint="/"`锛変笂杩愯锛屼笖涓昏溅鏈 Detour 瀹氫綅锛堢敤浜庡弽鎺ㄨ溅闃熶腑蹇冭捣鐐癸級銆 + +| 瀛楁锛坄clumsy.json` 鈫 `msConf`锛 | 鍚箟 | 榛樿鍊 | +|------|------|--------| +| `FleetCrabAngleDeg` | 锜硅璺緞**涓庡綋鍓嶈溅闃熸湞鍚戠殑澶硅**(deg锛岄嗘椂閽堜负姝)銆傚喅瀹氭枩琛屾柟鍚戯細0=姝ｅ墠鏂癸紝90=姝ｅ乏鏂瑰钩绉伙紝-90=姝ｅ彸鏂广傜ǔ鎬佷笅鍗冲悇鑸佃疆鐨勮煿琛岃 | `45` | +| `FleetCrabLengthMm` | 锜硅璺緞**闀垮害**(mm)锛屾部澶硅鏂瑰悜琛岄┒璇ヨ窛绂诲悗鍋滆溅缁撴潫 | `2000` | +| `FleetCrabSpeed` | 锜硅**琛岄┒閫熷害**(m/s) | `0.2` | +| `FleetCrabGcpThetaThreshold` | 鍏煎鏃у嚑浣曟帶鍒跺櫒瀹炵幇鐨 gcp 鑸佃涓婇檺锛涘綋鍓嶈剼鏈墜鍔ㄧ瓑浠峰疄鐜颁笉鐩存帴浣跨敤 | `95` | +| `FleetCrabCorrectionGain` | 妯悜璇樊绾犲亸澧炵泭銆傚澶у悗鏀舵暃鏇村揩锛屼絾鏇村鏄撳嚭鐜版柟鍚戞憜鍔 | `1` | +| `FleetCrabCorrectionAngleDeg` | 鑷姩绾犲亸鏈澶ф敼鍚戣(deg)銆傝秺灏忚秺鎺ヨ繎鎵嬪姩锜硅锛岃秺澶х籂鍋忚秺寮 | `8` | +| `FleetCrabCommandAccel` | `Vx/Vy` 鍛戒护鏂滅巼闄愬埗(m/s虏)锛屾姂鍒剁籂鍋忔柟鍚戠獊鍙 | `0.4` | +| `MultiVehicleCrabSteerLimitDeg` | mode=1 锜硅鑸佃涓婇檺锛屽簲涓 Medulla 渚 `WheelAngleLowerLimit/UpperLimit` 鍖归厤锛沗-95掳` 鍦ㄩ粯璁 120掳 鍐呬細鐩存帴涓嬪彂 | `120` | + +**琛屼负瑕佺偣 / 娉ㄦ剰** + +- 褰撳墠瀹炵幇涓嶈蛋 `MultiVehicleAuto*`/`MultiVehicleSendMotion`锛屼篃涓嶅啀涓存椂鏀 `MultiVehicleAutoUseIdealCenter`锛涚粨鏉/鎬ュ仠浼氭竻闆惰剼鏈瓧娈点 +- `FleetCrabDbg` 浼氳褰 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`锛沗MultiVehicleDbg` 鍙户缁鐓ф渶缁 `frontTh鈮坮earTh`銆佹槸鍚︽湁 POS/Detect 琛ュ伩锛屼互鍙 `CRAB in/raw/limit/rev` 鏄惁鍦ㄨ埖瑙掍笂闄愬唴淇濇寔杩炵画琛ㄨ揪銆 +- `MultiVehicleUseDetect=true` 鏃朵粛鍙 2 鑵挎娴嬪畨鍏ㄩ棬绾︽潫锛堟娴嬩涪澶变細琚疆闆跺仠杞︼級銆 +- Playground 鍙岃溅鍦烘櫙鐨 `actuator.maxSteeringAngle` 涔熷繀椤讳笌璇ヤ笂闄愪竴鑷达紱鑻ヤ粛涓 `90`锛孋lumsy 鍙戝嚭鐨 `-98掳` 绾犲亸浼氬湪浠跨湡鎵ц灞傝澶瑰洖 `-90掳`锛岃〃鐜颁负绾í绉昏矾寰勬棤娉曟敹鏁涖 + +--- + +## 7. 甯歌鍧戜綅锛堟帓鏌ユ竻鍗曪級 - **缁勪笉鎴愰槦 / `ready=false`**锛歚MultiVehicleFleetNum` 涓庡疄闄呰溅鏁颁笉绗︼紱浠庤溅 `MultiVehicleMasterEndpoint` 娌℃寚鍚戜富杞︾鍙ｏ紱`soTag` 涓嶅尮閰嶃 - **鎽囨潌鏃犲弽搴**锛歚MultiVehicleUseDetect=true` 浣嗘娴嬫病閿侊紙瀹夊叏闂級锛涙垨璇妸绯绘暟璁炬垚 <1 / 婊戞潯涔橀浂锛涙垨鍦ㄤ粠杞﹁岄潪涓昏溅涓婃搷浣溿 @@ -140,7 +181,7 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) --- -## 7. 鍙傝冿細Playground 鍙岃溅楠岃瘉鍊奸熸煡 +## 8. 鍙傝冿細Playground 鍙岃溅楠岃瘉鍊奸熸煡 ```jsonc // clumsy.json -> msConf 锛堜富杞=agv_multi_1锛涗粠杞︽妸鏍囨敞椤规敼涓鸿溅2鍊硷級 @@ -155,7 +196,17 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) "SyncThAccPerSec": 30, "MultiVehicleMasterEndpoint": "/", // 浠庤溅: "127.0.0.1:8008" "PlaygroundRobotName": "agv_multi_1", // 浠庤溅: "agv_multi_2" -"TwoLegLidarName": "rear_left_lidar_1,rear_right_lidar_1" // 浠庤溅: *_2 +"TwoLegLidarName": "rear_left_lidar_1,rear_right_lidar_1", // 浠庤溅: *_2 + +// 鑷姩锜硅鍔ㄤ綔锛堣 搂6锛屼粎涓昏溅瑙﹀彂锛 +"FleetCrabAngleDeg": 45, +"FleetCrabLengthMm": 2000, +"FleetCrabSpeed": 0.2, +"FleetCrabGcpThetaThreshold": 95, +"FleetCrabCorrectionGain": 1.0, +"FleetCrabCorrectionAngleDeg": 8, +"FleetCrabCommandAccel": 0.4, +"MultiVehicleCrabSteerLimitDeg": 120 ``` ```text diff --git a/docs/RunAndDeploy.md b/docs/RunAndDeploy.md index e768851..ba09721 100644 --- a/docs/RunAndDeploy.md +++ b/docs/RunAndDeploy.md @@ -77,7 +77,7 @@ Tutorial/ ## 6. 鍙岃溅鑷姩鑱斿姩 1. 瀹屾垚涓婁竴鑺傚弻杞﹀惎鍔 -2. 鑷冲皯涓鍙拌溅 `PosAvailable=true`锛圖etour 瀹氫綅鏈夋晥锛 +2. 鍙夛細`MultiVehicleSyncUseDetour=true` 寮鍚**杞﹂槦鍐呭Э鎬佺籂姝**锛圫LAM 閫愯溅 `PosBias*` 琛ュ伩锛夈傛敞鎰忓畠**涓嶅奖鍝**鏁磋溅闃熷Э鎬佽绠椻斺旇嚜鍔ㄦā寮忔棤璁鸿寮鍏冲浣曪紝涓昏溅閮界敤 Detour 鍙嶆帹杞﹂槦涓績锛屾棤鏈夋晥瀹氫綅鏃朵細闃诲鍋滆溅锛堣瑙 `MultiVehicleConfig.md` 搂5锛 3. 閫氳繃 Simple 璋冨害瑙﹀彂鑷姩鑱斿姩锛堟帹鑽愶級锛 - 缂栬瘧 `MultiWheelS`锛堣绗 2 鑺傦級锛岀‘璁 `build\Simple\plugins\MultiWheelS.dll` 瀛樺湪 - 鍦 `build\Simple\` 杩愯 `SimpleComposer.exe`锛堣嚜鍔ㄥ姞杞 `./plugins`锛