Fix FleetCrabWalk startup synchronization

2026-07-04 16:05:11 +08:00
parent 8e3f276a24
commit da3d59972c
6 changed files with 151 additions and 89 deletions
@@ -280,10 +280,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        if (!rotateActive)
        {
            _multiVehicleRotateModeActive = false;
-            MultiVehicleRotateWheelsReady = true;
+            if (!active)
+            {
+                MultiVehicleRotateWheelsReady = true;
+                _multiVehicleRotateAlignDetail = "";
+            }
            MultiVehicleRotateFleetReady = true;
            _multiVehicleRotateDirectionHint = 1f;
-            _multiVehicleRotateAlignDetail = "";
            return;
        }

@@ -1309,10 +1312,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _mvRotCompVx = 0;
                _mvRotCompVy = 0;
                _mvRotCompOmega = 0;
-                MultiVehicleRotateWheelsReady = true;
                MultiVehicleRotateFleetReady = true;
-                _multiVehicleRotateAlignDetail = "";
-                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
+                // Reset rotate-only PI state outside rotate mode.
                _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                _rotPiLastTime = DateTime.MinValue;
                _rotPoseEpisode = false;
@@ -1323,6 +1324,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
+                if (motionOk)
+                    MultiVehicleRotateWheelsReady = TryCheckRotateWheelAlignment(chassis,
+                        Math.Max(0.1f, Conf.FleetCrabStartWheelAlignDeg), out _multiVehicleRotateAlignDetail);
+                else
+                {
+                    MultiVehicleRotateWheelsReady = false;
+                    _multiVehicleRotateAlignDetail = chassis.LastMotionDecomposeFailureReason;
+                }
+                MultiVehicleRotateFleetReady = true;
                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
                if (!motionOk)
                {
@@ -1335,6 +1345,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                    $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
                    $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
+                    $"wheelReady={MultiVehicleRotateWheelsReady} align={_multiVehicleRotateAlignDetail} " +
                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
            }
        }
@@ -1631,15 +1642,64 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        return true;
    }

-    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
+    public long BeginFleetMotionWarmup()
    {
-        // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
-        // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
+        MultiVehicleRotateWheelsReady = false;
+        MultiVehicleRotateFleetReady = false;
+        _multiVehicleRotateAlignDetail = "fleet motion warmup pending";
+        return Interlocked.Read(ref _multiVehicleNotifySeq);
+    }
+
+    public bool IsFleetMotionWarmupReady(DateTime warmStartTime, long notificationSeqBaseline,
+        float syncTh, float syncDistance, out string detail)
+    {
+        var pending = new List<string>();
+        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<PilotConfig, PilotDefin
            MotionFeasible = _multiVehicleMotionFeasible,
            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
            RotateWheelsAligned = MultiVehicleRotateWheelsReady,
-            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail,
+            AppliedNotificationSeq = _multiVehicleAppliedSeq
        };
    }