From da3d59972c1b17e81b43ba062bf88e319caa6619 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Sat, 4 Jul 2026 15:57:35 +0800 Subject: [PATCH] Fix FleetCrabWalk startup synchronization --- MultiWheel/MultiWheelC/AGV.cs | 36 ++++--- MultiWheel/MultiWheelC/MovementTests.cs | 94 ++++++++----------- MultiWheel/MultiWheelC/PilotConfig.cs | 6 ++ MultiWheel/MultiWheelC/PilotDefinition.cs | 83 +++++++++++++--- .../MultiWheelC/VehicleSyncBinaryCodec.cs | 20 ++-- MultiWheel/MultiWheelC/VehicleSyncModels.cs | 1 + 6 files changed, 151 insertions(+), 89 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 603fea8..8d5713f 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -2,6 +2,7 @@ using ClumsyCore; using ClumsyCore.Interfaces; using ClumsyCore.Sensors; using CommonUsage.Chassis; +using CommonUsage.Mathematics; using FundamentalLib; using MDCSToolBox.Clumsy.AgvInterfaces; using MDCSToolBox.Clumsy.MotionControllers; @@ -292,18 +293,13 @@ namespace MultiWheelC RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos }.Get()).Wait(); } - // 车队联动-自动蟹行：供调度按绝对起终点触发。 - // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。 + // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest. public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId, - float speed, float CarDirectionBias) + float speed) { 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) { @@ -311,6 +307,23 @@ namespace MultiWheelC return; } + var self = PilotDefinition.Self; + if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) && + !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh)) + { + DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg"); + Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab"); + return; + } + + var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0)); + var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh); + 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}, pathAngle={pathAngle:0.0}, " + + $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0})", + "FleetCrabDbg"); + if (dstId != -1) { while (!TryLock(dstId)) @@ -322,14 +335,10 @@ namespace MultiWheelC var action = new MultiWheelC.FleetCrabWalk { - UseAbsolutePath = true, - PathStartX = srcX, - PathStartY = srcY, - PathEndX = dstX, - PathEndY = dstY, + CrabAngleDeg = crabAngle, + BodyToPathAngleDeg = crabAngle, CrabLengthMm = pathLength, CrabSpeed = speed, - BodyToPathAngleDeg = -CarDirectionBias, FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel, FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance, FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance, @@ -352,7 +361,6 @@ namespace MultiWheelC } } - public void ChangeAvoidanceDistance(float stopDistance, float slowDistance) { DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})"); diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index e03bbcf..e42fa83 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -447,14 +447,6 @@ 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; @@ -663,7 +655,7 @@ 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 pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " + + $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " + $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " + $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}", "FleetCrabDbg"); @@ -687,40 +679,17 @@ public class FleetCrabWalk : MovementDefinition DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg"); - 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 pathStart = new Vector2(x0, y0); + var pathLengthMm = CrabLengthMm; + var phi = CommonMath.RoundTh(theta + CrabAngleDeg); + var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0)); var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg); 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}) pathMode={(UseAbsolutePath ? "absolute" : "relative")} " + + $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=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={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " + @@ -728,6 +697,16 @@ public class FleetCrabWalk : MovementDefinition $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}", "FleetCrabDbg"); + var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); + var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0 + ? conf.MultiVehicleControlRadius + : conf.TestCarSyncDistance / 2f)); + ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta), + (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh, + out _, out _); + var warmStart = DateTime.Now; + var warmSeqBaseline = self.BeginFleetMotionWarmup(); + self.MultiVehicleScriptEnabled = false; self.MultiVehicleScriptMode = 0; self.MultiVehicleScriptVx = 0; @@ -735,19 +714,23 @@ public class FleetCrabWalk : MovementDefinition self.MultiVehicleScriptVth = 0; self.MultiVehicleAutoEnabled = true; self.MultiVehicleAutoVx = 0; - self.MultiVehicleAutoFrontTh = 0; - self.MultiVehicleAutoRearTh = 0; + self.MultiVehicleAutoFrontTh = holdFrontTh; + self.MultiVehicleAutoRearTh = holdRearTh; self.MultiVehicleAutoIdealX = pathStart.X; self.MultiVehicleAutoIdealY = pathStart.Y; self.MultiVehicleAutoIdealTh = targetBodyTh; self.MultiVehicleAutoHasIdeal = true; self.MultiVehicleAutoCmdTime = DateTime.Now; self.PrimeMasterAutoFromSlam(); - DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg"); + DLog.Log( + $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " + + $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})", + "FleetCrabDbg"); - var warmEnd = DateTime.Now.AddSeconds(2.0); + var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec)); var warmIter = 0; var warmReady = false; + var warmDetail = ""; while (!_stopping && DateTime.Now < warmEnd) { warmIter++; @@ -755,8 +738,8 @@ public class FleetCrabWalk : MovementDefinition self.MultiVehicleScriptMode = 0; self.MultiVehicleAutoEnabled = true; self.MultiVehicleAutoVx = 0; - self.MultiVehicleAutoFrontTh = 0; - self.MultiVehicleAutoRearTh = 0; + self.MultiVehicleAutoFrontTh = holdFrontTh; + self.MultiVehicleAutoRearTh = holdRearTh; self.MultiVehicleAutoIdealX = pathStart.X; self.MultiVehicleAutoIdealY = pathStart.Y; self.MultiVehicleAutoIdealTh = targetBodyTh; @@ -769,26 +752,32 @@ public class FleetCrabWalk : MovementDefinition if (warmIter % 5 == 0) DLog.Log( $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " + - $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}", + $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " + + $"detail={warmDetail}", "FleetCrabDbg"); - if (cnt >= conf.MultiVehicleFleetNum) + if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline, + conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail)) { warmReady = true; DLog.Log( - $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}", + $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}", "FleetCrabDbg"); break; } yield return true; } if (!warmReady) - DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.", + { + DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}", "FleetCrabDbg"); + Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab"); + Cleanup(); + yield break; + } Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab"); - if (warmReady && !UseAbsolutePath && - self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh)) + if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh)) { x0 = warmX; y0 = warmY; @@ -819,13 +808,6 @@ public class FleetCrabWalk : MovementDefinition var accel = Math.Abs(FleetCrabAccel); var cmdSpeed = 0f; var lastTick = DateTime.Now; - var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); - var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0 - ? conf.MultiVehicleControlRadius - : conf.TestCarSyncDistance / 2f)); - ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta), - (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh, - out _, out _); var stopReason = "done"; while (!_stopping) diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 71b58bf..8b878b9 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -185,6 +185,12 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")] public float FleetCrabGcpThetaThreshold = 95f; + [FieldMember(desc = "FleetCrab startup sync timeout(s)")] + public float FleetCrabStartSyncTimeoutSec = 8f; + + [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")] + public float FleetCrabStartWheelAlignDeg = 2f; + // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）===== [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 604e238..5c83c8e 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -280,10 +280,13 @@ public class PilotDefinition : MultiWheelPilotDefinition(); + var requiredCount = Math.Max(1, Conf.MultiVehicleFleetNum); + lock (FleetLock) + { + if (MultiVehicleFleet.Count != requiredCount) + pending.Add($"fleetCnt={MultiVehicleFleet.Count}/{requiredCount}"); + + foreach (var kv in MultiVehicleFleet.OrderBy(k => k.Key)) + { + var carNum = kv.Key; + var info = kv.Value; + if (!_multiVehicleFleetSeen.TryGetValue(carNum, out var seen) || seen < warmStartTime) + pending.Add($"car{carNum}:notFresh"); + + var (layoutX, layoutY, layoutTh) = GetLayoutPoseForCar(carNum, syncTh, syncDistance); + if (Math.Abs(info.LayoutX - layoutX) > 1f || + Math.Abs(info.LayoutY - layoutY) > 1f || + Math.Abs(CommonMath.ThDiff(info.LayoutTh, layoutTh)) > 1f) + pending.Add( + $"car{carNum}:layout=({info.LayoutX:0},{info.LayoutY:0},{info.LayoutTh:0.0})"); + + if (!info.MotionFeasible) + pending.Add($"car{carNum}:motion={info.MotionInfeasibleReason}"); + if (!info.RotateWheelsAligned) + pending.Add($"car{carNum}:wheel={info.RotateWheelAlignDetail}"); + if (carNum != CarNum && info.AppliedNotificationSeq <= notificationSeqBaseline) + pending.Add($"car{carNum}:seq={info.AppliedNotificationSeq}<={notificationSeqBaseline}"); + } + } + + detail = pending.Count == 0 + ? $"ready seqBase={notificationSeqBaseline}" + : string.Join("; ", pending); + return pending.Count == 0; + } + + private (float, float, float) GetLayoutPose(float syncTh, float syncDistance) + => GetLayoutPoseForCar(CarNum, syncTh, syncDistance); + + private static (float, float, float) GetLayoutPoseForCar(int carNum, float syncTh, float syncDistance) + { + // Two-car layout: members are mirrored around the fleet center; car2 faces 180 deg away. var rad = syncTh / 180f * Math.PI; - var sign = CarNum == 1 ? 1f : -1f; + var sign = carNum == 1 ? 1f : -1f; var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign); var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign); - return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180)); + return (xx, yy, syncTh + (carNum == 1 ? 0 : 180)); } private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th, @@ -1664,7 +1724,8 @@ public class PilotDefinition : MultiWheelPilotDefinition Version) throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}."); var type = reader.ReadByte(); @@ -163,6 +163,8 @@ internal static class VehicleSyncBinaryCodec var reserved = reader.ReadUInt16(); if (reserved != 0) throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field."); + + return version; } private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info) @@ -178,9 +180,10 @@ internal static class VehicleSyncBinaryCodec writer.Write(info.LayoutTh); WriteString(writer, info.MotionInfeasibleReason); WriteString(writer, info.RotateWheelAlignDetail); + writer.Write(info.AppliedNotificationSeq); } - private static VehicleSyncInfo ReadInfo(BinaryReader reader) + private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version) { var info = new VehicleSyncInfo(); ApplyInfoFlags(info, reader.ReadUInt16()); @@ -194,6 +197,7 @@ internal static class VehicleSyncBinaryCodec info.LayoutTh = reader.ReadSingle(); info.MotionInfeasibleReason = ReadString(reader); info.RotateWheelAlignDetail = ReadString(reader); + info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1; return info; } diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs index 9f5ca3f..1c21428 100644 --- a/MultiWheel/MultiWheelC/VehicleSyncModels.cs +++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs @@ -23,6 +23,7 @@ public class VehicleSyncInfo [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = ""; [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true; [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = ""; + [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1; } public class VehicleSyncNotification