Add absolute fleet crab walk path support

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))

MultiWheel/MultiWheelC/MovementTests.cs

@@ -447,6 +447,14 @@ 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;
 
@@ -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} " +
+            $"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " +
-            $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
+            $"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.MultiVehicleAutoIdealX = pathStart.X;
-        self.MultiVehicleAutoIdealY = y0;
+        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.MultiVehicleAutoIdealX = pathStart.X;
-            self.MultiVehicleAutoIdealY = y0;
+            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 idealAlong = Clamp(along, 0f, pathLengthMm);
-            var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
+            var ideal = pathStart + pathDir * idealAlong;
 
             self.MultiVehicleScriptEnabled = false;
             self.MultiVehicleScriptMode = 0;