From cb104b13cc84e89d30ab150802d66ecf358fc088 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Thu, 2 Jul 2026 17:17:32 +0800 Subject: [PATCH] Add absolute fleet crab walk path support --- MultiWheel/MultiWheelC/AGV.cs | 59 ++++++++++++++++++++++ MultiWheel/MultiWheelC/MovementTests.cs | 65 +++++++++++++++++++------ 2 files changed, 109 insertions(+), 15 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 8e06b04..b022503 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -226,6 +226,65 @@ namespace MultiWheelC RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos }.Get()).Wait(); } + // 车队联动-自动蟹行：供调度按绝对起终点触发。 + // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。 + public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId, + float speed, float CarDirectionBias) + { + var dx = dstX - srcX; + var dy = dstY - srcY; + var pathLength = (float)Math.Sqrt(dx * dx + dy * dy); + DLog.Log( + $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " + + $"len={pathLength:0.0}, speed={speed:0.000}, CarDirectionBias={CarDirectionBias:0.0})", + "FleetCrabDbg"); + + if (pathLength <= 1f) + { + DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg"); + return; + } + + if (dstId != -1) + { + while (!TryLock(dstId)) + { + Thread.Sleep(50); + } + DLog.Log($"锁点{dstId}完成", "FleetCrabDbg"); + } + + var action = new MultiWheelC.FleetCrabWalk + { + UseAbsolutePath = true, + PathStartX = srcX, + PathStartY = srcY, + PathEndX = dstX, + PathEndY = dstY, + CrabLengthMm = pathLength, + CrabSpeed = speed, + BodyToPathAngleDeg = -CarDirectionBias, + FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel, + FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance, + FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance, + FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed, + FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow, + GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold + }; + + try + { + new DriveTask(action.Get()).Wait(); + } + finally + { + if (srcId != -1) + { + Leave(srcId); + DLog.Log($"释放放车点{srcId}", "FleetCrabDbg"); + } + } + } public void LineTracking(int srcId, int dstId) { while (!TryLock(dstId)) diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index 34c0f26..f0a819a 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -447,6 +447,14 @@ public class FleetCrabWalk : MovementDefinition /// 璺緞闀垮害(mm)銆 public float CrabLengthMm = 2000f; + /// 浣跨敤璋冨害缁欏畾鐨勭粷瀵硅捣缁堢偣璺緞锛岃屼笉鏄敤褰撳墠杞﹂槦涓績鍜 CrabAngleDeg 鐢熸垚璺緞銆 + public bool UseAbsolutePath = false; + + public float PathStartX; + public float PathStartY; + public float PathEndX; + public float PathEndY; + /// 琛岄┒閫熷害(m/s)銆 public float CrabSpeed = 0.2f; @@ -525,8 +533,9 @@ public class FleetCrabWalk : MovementDefinition $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " + $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " + $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " + - $"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " + - $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}", + $"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " + + $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " + + $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}", "FleetCrabDbg"); if (conf.MultiVehicleMasterEndpoint != "/") @@ -546,17 +555,43 @@ public class FleetCrabWalk : MovementDefinition } DLog.Log($"CENTER 杞﹂槦涓績=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg"); - var phi = CommonMath.RoundTh(theta + CrabAngleDeg); + Vector2 pathStart; + Vector2 dst; + float pathLengthMm; + float phi; + if (UseAbsolutePath) + { + pathStart = new Vector2(PathStartX, PathStartY); + dst = new Vector2(PathEndX, PathEndY); + var pathVec = dst - pathStart; + pathLengthMm = pathVec.Length(); + if (pathLengthMm <= 1f) + { + DLog.Log( + $"ABORT: absolute path is too short src=({PathStartX:0},{PathStartY:0}) dst=({PathEndX:0},{PathEndY:0}) len={pathLengthMm:0.0}", + "FleetCrabDbg"); + Hedingben.ToastText("杞﹂槦锜硅璺緞闀垮害杩囩煭", "FleetCrab"); + yield break; + } + phi = CommonMath.RoundTh((float)(Math.Atan2(pathVec.Y, pathVec.X) / Math.PI * 180.0)); + } + else + { + pathStart = new Vector2(x0, y0); + pathLengthMm = CrabLengthMm; + phi = CommonMath.RoundTh(theta + CrabAngleDeg); + dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0)); + } var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg); - 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}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " + + $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode={(UseAbsolutePath ? "absolute" : "relative")} " + + $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " + $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " + - $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " + + $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " + $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " + $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}", "FleetCrabDbg"); @@ -570,8 +605,8 @@ public class FleetCrabWalk : MovementDefinition self.MultiVehicleAutoVx = 0; self.MultiVehicleAutoFrontTh = 0; self.MultiVehicleAutoRearTh = 0; - self.MultiVehicleAutoIdealX = x0; - self.MultiVehicleAutoIdealY = y0; + self.MultiVehicleAutoIdealX = pathStart.X; + self.MultiVehicleAutoIdealY = pathStart.Y; self.MultiVehicleAutoIdealTh = targetBodyTh; self.MultiVehicleAutoHasIdeal = true; self.MultiVehicleAutoCmdTime = DateTime.Now; @@ -590,8 +625,8 @@ public class FleetCrabWalk : MovementDefinition self.MultiVehicleAutoVx = 0; self.MultiVehicleAutoFrontTh = 0; self.MultiVehicleAutoRearTh = 0; - self.MultiVehicleAutoIdealX = x0; - self.MultiVehicleAutoIdealY = y0; + self.MultiVehicleAutoIdealX = pathStart.X; + self.MultiVehicleAutoIdealY = pathStart.Y; self.MultiVehicleAutoIdealTh = targetBodyTh; self.MultiVehicleAutoHasIdeal = true; self.MultiVehicleAutoCmdTime = DateTime.Now; @@ -618,7 +653,7 @@ public class FleetCrabWalk : MovementDefinition DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.", "FleetCrabDbg"); - Hedingben.ToastText($"杞﹂槦锜硅 璺緞{CrabAngleDeg:0.0}掳 杞﹁韩澶硅{BodyToPathAngleDeg:0.0}掳 闀垮害{CrabLengthMm:0}mm", "FleetCrab"); + Hedingben.ToastText($"杞﹂槦锜硅 璺緞{phi:0.0}掳 杞﹁韩澶硅{BodyToPathAngleDeg:0.0}掳 闀垮害{pathLengthMm:0}mm", "FleetCrab"); var iter = 0; var lastLog = DateTime.MinValue; @@ -645,10 +680,10 @@ public class FleetCrabWalk : MovementDefinition DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg"); break; } - var delta = new Vector2(cx - x0, cy - y0); + var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y); var along = Vector2.Dot(delta, pathDir); var lateral = Vector2.Dot(delta, pathLeft); - var remain = CrabLengthMm - along; + var remain = pathLengthMm - along; if (remain <= finishDistance) break; @@ -674,8 +709,8 @@ public class FleetCrabWalk : MovementDefinition var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit); holdFrontTh = frontTh; holdRearTh = rearTh; - var idealAlong = Clamp(along, 0f, CrabLengthMm); - var ideal = new Vector2(x0, y0) + pathDir * idealAlong; + var idealAlong = Clamp(along, 0f, pathLengthMm); + var ideal = pathStart + pathDir * idealAlong; self.MultiVehicleScriptEnabled = false; self.MultiVehicleScriptMode = 0;