Fix FleetCrabWalk startup synchronization

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})");