Fix FleetCrabWalk startup synchronization

MultiWheel/MultiWheelC/MovementTests.cs

@@ -447,14 +447,6 @@ public class FleetCrabWalk : MovementDefinition
     /// <summary>路径长度(mm)。</summary>
     public float CrabLengthMm = 2000f;
 
-    /// <summary>使用调度给定的绝对起终点路径，而不是用当前车队中心和 CrabAngleDeg 生成路径。</summary>
-    public bool UseAbsolutePath = false;
-
-    public float PathStartX;
-    public float PathStartY;
-    public float PathEndX;
-    public float PathEndY;
-
     /// <summary>行驶速度(m/s)。</summary>
     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)