Add fleet curve walk support

MultiWheel/MultiWheelC/AGV.cs

@@ -401,6 +401,102 @@ namespace MultiWheelC
             }
         }
 
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
+        {
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+            {
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
+            {
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+                return;
+            }
+
+            var controlPointNum = (int)trackTypeInfo[1];
+            var expectedLength = 2 + controlPointNum * 2;
+            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+            {
+                DLog.Log(
+                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            BezierTrack track;
+            try
+            {
+                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+            }
+            catch (Exception ex)
+            {
+                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+                return;
+            }
+
+            if (track == null)
+            {
+                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+                return;
+            }
+
+            track.Speed = speed;
+            track.CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias;
+
+            DLog.Log(
+                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+                $"carDirectionBias={PilotDefinition.Conf.FleetCurveCarDirectionBias:0.0})",
+                "FleetCurveDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
+            }
+
+            var action = new MultiWheelC.FleetCurveWalk
+            {
+                Track = track,
+                CurveSpeed = speed,
+                CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
+                BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
+                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"閲婃斁鏀捐溅鐐箋srcId}", "FleetCurveDbg");
+                }
+            }
+        }
+        
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
         {
             DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");

MultiWheel/MultiWheelC/FleetCrabWalk.cs

@@ -0,0 +1,528 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
+//
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    public float CrabAngleDeg = 45f;
+
+    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public float BodyToPathAngleDeg = 45f;
+
+    /// <summary>路径长度(mm)。</summary>
+    public float CrabLengthMm = 2000f;
+
+    /// <summary>行驶速度(m/s)。</summary>
+    public float CrabSpeed = 0.2f;
+
+    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabAccel = 0.2f;
+
+    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabStartAccel = 0.01f;
+
+    /// <summary>末端开始减速距离(mm)。</summary>
+    public float FleetCrabSlowDistance = 2000f;
+
+    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
+    public float FleetCrabFinishDistance = 20f;
+
+    /// <summary>末端最低速度(m/s)。</summary>
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    /// <summary>末端减速曲线指数。</summary>
+    public float FleetCrabSlowingPow = 0.8f;
+
+    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
+    public float GcpThetaThreshold = 95f;
+
+    private bool _stopping;
+
+    private void Cleanup()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoHasIdeal = false;
+        self.MultiVehicleAutoEnabled = false;
+    }
+
+    public void Stop()
+    {
+        _stopping = true;
+        Cleanup();
+    }
+
+    private static float Clamp(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static float Slew(float current, float target, float maxDelta)
+    {
+        if (maxDelta <= 0) return target;
+        if (target > current + maxDelta) return current + maxDelta;
+        if (target < current - maxDelta) return current - maxDelta;
+        return target;
+    }
+
+    private static float AverageAngle(float frontTh, float rearTh)
+    {
+        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+    }
+
+    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+    {
+        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+        driveSpeed = speed;
+        frontTh = rawFrontTh;
+        rearTh = rawRearTh;
+        reverseEquivalent = false;
+
+        if (rawBaseTh > limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+        else if (rawBaseTh < -limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+
+        frontTh = ClampAbs(frontTh, limit);
+        rearTh = ClampAbs(rearTh, limit);
+    }
+
+    private static float ProbeSpeed(float speed)
+    {
+        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+    }
+
+    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+        out float yawSign)
+    {
+        yawSign = 0f;
+        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+            return false;
+
+        var radius = Math.Max(1f, Math.Abs(controlRadius));
+        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+        if (!intersect)
+            return false;
+
+        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+        var tangentSign = center.Y > 1f ? 1f : -1f;
+        var speedSign = driveSpeed >= 0f ? 1f : -1f;
+        yawSign = speedSign * tangentSign;
+        return true;
+    }
+
+    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+    {
+        const float probeDth = 1f;
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+            ? yawSign
+            : 1f;
+    }
+
+    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+        Vector2 pathLeft)
+    {
+        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+        var rad = motionTh / 180f * Math.PI;
+        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+        if (driveSpeed < 0f)
+            dir = -dir;
+        return Vector2.Dot(dir, pathLeft);
+    }
+
+    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral, float biasProbe)
+    {
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+        return -Math.Sign(lateral) * lateralVelocity;
+    }
+
+    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral)
+    {
+        if (Math.Abs(lateral) <= 1e-3f)
+            return 1f;
+
+        const float probeBias = 1f;
+        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+        return positiveScore >= negativeScore ? 1f : -1f;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+            yield break;
+        }
+        _stopping = false;
+
+        DLog.Log(
+            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
+            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
+            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
+            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
+            "FleetCrabDbg");
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
+            yield break;
+        }
+
+        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
+        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+            yield break;
+        }
+        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        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=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} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
+            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+            $"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;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 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 startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");
+
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
+        var warmIter = 0;
+        var warmReady = false;
+        var warmDetail = "";
+        while (!_stopping && DateTime.Now < warmEnd)
+        {
+            warmIter++;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 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();
+            var snap = self.GetFleetCenterSnapshot();
+            int cnt;
+            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+            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} " +
+                    $"detail={warmDetail}",
+                    "FleetCrabDbg");
+            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} detail={warmDetail}",
+                    "FleetCrabDbg");
+                break;
+            }
+            yield return true;
+        }
+        if (!warmReady)
+        {
+            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 && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        {
+            x0 = warmX;
+            y0 = warmY;
+            theta = warmTh;
+            pathStart = new Vector2(x0, y0);
+            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+            phiRad = phi / 180.0 * Math.PI;
+            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            DLog.Log(
+                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+                "FleetCrabDbg");
+        }
+
+        var iter = 0;
+        var lastLog = DateTime.MinValue;
+        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
+        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
+        var baseSpeed = Math.Abs(CrabSpeed);
+        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
+        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+        var accel = Math.Abs(FleetCrabAccel);
+        var startAccel = Math.Abs(FleetCrabStartAccel);
+        var cmdSpeed = 0f;
+        var lastTick = DateTime.Now;
+        var speedRampStart = DateTime.Now;
+        var stopReason = "done";
+
+        while (!_stopping)
+        {
+            iter++;
+
+            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
+            {
+                stopReason = "fleet center invalid";
+                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
+                break;
+            }
+            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
+            var along = Vector2.Dot(delta, pathDir);
+            var lateral = Vector2.Dot(delta, pathLeft);
+            var remain = pathLengthMm - along;
+            if (remain <= finishDistance)
+                break;
+
+            var targetSpeed = baseSpeed;
+            var slowRatio = 1f;
+            if (remain < slowDistance)
+            {
+                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
+            }
+            var now = DateTime.Now;
+            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+            lastTick = now;
+            var rampElapsed = (now - speedRampStart).TotalSeconds;
+            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
+            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
+            cmdSpeed = speed;
+
+            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+            var rawBiasItem = rawBiasMagnitude * biasSign;
+            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
+            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
+            var rawFrontTh = baseCrabTh + biasItem + dthItem;
+            var rawRearTh = baseCrabTh + biasItem - dthItem;
+            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
+            holdFrontTh = frontTh;
+            holdRearTh = rearTh;
+            var idealAlong = Clamp(along, 0f, pathLengthMm);
+            var ideal = pathStart + pathDir * idealAlong;
+
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = driveSpeed;
+            self.MultiVehicleAutoFrontTh = frontTh;
+            self.MultiVehicleAutoRearTh = rearTh;
+            self.MultiVehicleAutoIdealX = ideal.X;
+            self.MultiVehicleAutoIdealY = ideal.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+
+            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
+            {
+                lastLog = DateTime.Now;
+                var snap = self.GetFleetCenterSnapshot();
+                int fleetCnt;
+                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
+                DLog.Log(
+                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
+                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
+                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
+                    "FleetCrabDbg");
+                DLog.Log(
+                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
+                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
+                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
+                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
+                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
+                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
+                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    "FleetCrabHeadingDbg");
+            }
+            yield return true;
+        }
+
+        if (_stopping)
+            stopReason = "stop";
+
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        DLog.Log(
+            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
+            "FleetCrabDbg");
+        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+        while (!_stopping && DateTime.Now < settleEnd)
+        {
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            yield return true;
+        }
+
+        Cleanup();
+        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
+        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
+    }
+}

MultiWheel/MultiWheelC/FleetCurveWalk.cs

@@ -0,0 +1,413 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+
+namespace MultiWheelC;
+
+public class FleetCurveWalk : MovementDefinition
+{
+    public BezierTrack Track;
+    public List<Vector2> ControlPoints = new();
+    public float CurveSpeed = 0.2f;
+    public float CarDirectionBias = 0f;
+    public int BezierResolution = 100;
+    public float SlowDistance = 2000f;
+    public float FinishDistance = 20f;
+    public float FinishSpeed = 0.02f;
+    public float SlowingPow = 0.8f;
+    public float GcpThetaThreshold = 95f;
+    public float StartSyncTimeoutSec = 8f;
+
+    private bool _stopping;
+    private MultiWheelGeometricController _controller;
+    private MultiWheelChassis _chassis;
+    private bool _savedControlPoints;
+    private float _savedControlRadius;
+    private Vector2 _savedGcp0;
+    private Vector2 _savedGcp1;
+
+    public void Stop()
+    {
+        _stopping = true;
+        if (_controller != null)
+            _controller.BreakAndHold = true;
+        Cleanup();
+    }
+
+    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
+    {
+        points = new List<Vector2>();
+        error = "";
+        if (string.IsNullOrWhiteSpace(text))
+        {
+            error = "empty control point list";
+            return false;
+        }
+
+        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
+        for (var i = 0; i < segments.Length; i++)
+        {
+            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
+            if (pair.Length != 2)
+            {
+                error = $"invalid point #{i + 1}: {segments[i]}";
+                return false;
+            }
+
+            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
+            {
+                error = $"invalid number in point #{i + 1}: {segments[i]}";
+                return false;
+            }
+            points.Add(new Vector2(x, y));
+        }
+
+        if (points.Count < 3)
+        {
+            error = "Bezier curve requires at least 3 control points";
+            return false;
+        }
+
+        return true;
+    }
+
+    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
+    {
+        var source = relativePoints ?? new List<Vector2>();
+        var normalized = new List<Vector2>();
+        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
+            normalized.Add(Vector2.Zero);
+        for (var i = 0; i < source.Count; i++)
+            normalized.Add(source[i]);
+        if (normalized.Count < 2)
+            normalized.Add(new Vector2(1000f, 0f));
+        if (normalized.Count < 3)
+            normalized.Add(new Vector2(2000f, 0f));
+
+        var result = new List<Vector2>();
+        for (var i = 0; i < normalized.Count; i++)
+            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
+        return result;
+    }
+
+    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
+        params float[] controlPointCoords)
+    {
+        var src = new Vector2(srcX, srcY);
+        var dst = new Vector2(dstX, dstY);
+        var result = new List<Vector2>();
+        if (controlPointCoords == null || controlPointCoords.Length == 0)
+        {
+            result.Add(src);
+            result.Add((src + dst) / 2f);
+            result.Add(dst);
+            return result;
+        }
+
+        if (controlPointCoords.Length % 2 != 0)
+            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
+
+        var supplied = new List<Vector2>();
+        for (var i = 0; i < controlPointCoords.Length; i += 2)
+            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
+
+        if (supplied.Count >= 3 &&
+            Vector2.Distance(supplied[0], src) <= 10f &&
+            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
+            return supplied;
+
+        result.Add(src);
+        for (var i = 0; i < supplied.Count; i++)
+            result.Add(supplied[i]);
+        result.Add(dst);
+        if (result.Count < 3)
+            result.Insert(1, (src + dst) / 2f);
+        return result;
+    }
+
+    private static bool TryParseFloat(string text, out float value)
+    {
+        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
+               float.TryParse(text, out value);
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    private void Cleanup()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoHasIdeal = false;
+        self.MultiVehicleAutoEnabled = false;
+        RestoreControlPointRadius();
+    }
+
+    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
+    {
+        if (!_savedControlPoints)
+        {
+            _chassis = chassis;
+            _savedControlRadius = chassis.ControlPointRadius;
+            var gcps = chassis.GetGeometricControlPoints();
+            if (gcps.Count >= 2)
+            {
+                _savedGcp0 = gcps[0].Position;
+                _savedGcp1 = gcps[1].Position;
+            }
+            _savedControlPoints = true;
+        }
+
+        chassis.ControlPointRadius = radius;
+        var points = chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = new Vector2(radius, 0);
+            points[1].Position = new Vector2(-radius, 0);
+        }
+    }
+
+    private void RestoreControlPointRadius()
+    {
+        if (!_savedControlPoints || _chassis == null)
+            return;
+
+        _chassis.ControlPointRadius = _savedControlRadius;
+        var points = _chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = _savedGcp0;
+            points[1].Position = _savedGcp1;
+        }
+        _savedControlPoints = false;
+    }
+
+    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
+        float frontTh, float rearTh)
+    {
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = frontTh;
+        self.MultiVehicleAutoRearTh = rearTh;
+        self.MultiVehicleAutoIdealX = idealPos.X;
+        self.MultiVehicleAutoIdealY = idealPos.Y;
+        self.MultiVehicleAutoIdealTh = idealTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        _stopping = false;
+
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
+            yield break;
+        }
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
+                "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
+            yield break;
+        }
+
+        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
+            yield break;
+        }
+
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+            yield break;
+        }
+
+        var baseSpeed = Math.Abs(CurveSpeed);
+        if (baseSpeed <= 1e-4f)
+        {
+            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
+            yield break;
+        }
+
+        var resolution = Math.Max(2, BezierResolution);
+        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+            ? conf.MultiVehicleControlRadius
+            : conf.TestCarSyncDistance / 2f));
+
+        ApplyFleetControlPointRadius(chassis, controlRadius);
+
+        try
+        {
+            var track = Track;
+            var trackSource = "external";
+            if (track == null)
+            {
+                var points = new List<Vector2>(ControlPoints);
+                track = new BezierTrack(points, resolution);
+                trackSource = "controlPoints";
+            }
+            track.CarDirectionBias = CarDirectionBias;
+            track.Speed = baseSpeed;
+
+            var center = new Vector2(x0, y0);
+            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
+            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
+            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
+            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
+
+            DLog.Log(
+                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
+                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
+                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
+                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
+                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
+                "FleetCurveDbg");
+
+            var warmStart = DateTime.Now;
+            var warmSeqBaseline = self.BeginFleetMotionWarmup();
+            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+            self.PrimeMasterAutoFromSlam();
+
+            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
+            var warmIter = 0;
+            var warmReady = false;
+            var warmDetail = "";
+            while (!_stopping && DateTime.Now < warmEnd)
+            {
+                warmIter++;
+                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+                self.PrimeMasterAutoFromSlam();
+
+                if (warmIter % 5 == 0)
+                {
+                    var snap = self.GetFleetCenterSnapshot();
+                    int cnt;
+                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+                    DLog.Log(
+                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
+                        "FleetCurveDbg");
+                }
+
+                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+                {
+                    warmReady = true;
+                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
+                    break;
+                }
+                yield return true;
+            }
+
+            if (!warmReady)
+            {
+                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
+                Cleanup();
+                yield break;
+            }
+
+            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
+            _controller.MultiVehicleSync = true;
+            _controller.BaseSpeed = baseSpeed;
+            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
+            _controller.FinishDistance = Math.Max(0f, FinishDistance);
+            _controller.FinishSpeed = speedFinish;
+            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
+            _controller.GcpThetaThreshold = gcpLimit;
+            _controller.AddTrack(track, "FleetCurve");
+
+            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
+
+            foreach (var running in _controller.Track())
+            {
+                if (_stopping)
+                    break;
+                if (!running)
+                    break;
+                yield return true;
+            }
+
+            if (!_stopping)
+            {
+                self.MultiVehicleAutoVx = 0;
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+                while (!_stopping && DateTime.Now < settleEnd)
+                {
+                    self.MultiVehicleAutoEnabled = true;
+                    self.MultiVehicleAutoVx = 0;
+                    self.MultiVehicleAutoCmdTime = DateTime.Now;
+                    yield return true;
+                }
+            }
+
+            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
+        }
+        finally
+        {
+            Cleanup();
+            _controller = null;
+        }
+    }
+}

MultiWheel/MultiWheelC/MovementTests.cs

@@ -7,10 +7,8 @@ using ClumsyCore.Pilot;
 using FundamentalLib;
 using CommonUsage.Chassis;
 using CommonUsage.Mathematics;
-using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Clumsy.Tracks;
 
 namespace MultiWheelC;
 
@@ -426,518 +424,53 @@ public class FleetRotateInPlaceTest : MovementTest
     }
 }
 
-// ===== 车队联动-自动蟹行动作 =====
+[MovementTest(name = "车队联动-曲线行走")]
-// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+public class FleetCurveWalkTest : MovementTest
-// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
-//
-// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
-// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
-// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
-// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
-//
-// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
-public class FleetCrabWalk : MovementDefinition
 {
-    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    private FleetCurveWalk _proc;
-    public float CrabAngleDeg = 45f;
+    private DriveTask _task;
 
-    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public override void Test()
-    public float BodyToPathAngleDeg = 45f;
-
-    /// <summary>路径长度(mm)。</summary>
-    public float CrabLengthMm = 2000f;
-
-    /// <summary>行驶速度(m/s)。</summary>
-    public float CrabSpeed = 0.2f;
-
-    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
-    public float FleetCrabAccel = 0.2f;
-
-    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
-    public float FleetCrabStartAccel = 0.01f;
-
-    /// <summary>末端开始减速距离(mm)。</summary>
-    public float FleetCrabSlowDistance = 2000f;
-
-    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
-    public float FleetCrabFinishDistance = 20f;
-
-    /// <summary>末端最低速度(m/s)。</summary>
-    public float FleetCrabFinishSpeed = 0.02f;
-
-    /// <summary>末端减速曲线指数。</summary>
-    public float FleetCrabSlowingPow = 0.8f;
-
-    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
-    public float GcpThetaThreshold = 95f;
-
-    private bool _stopping;
-
-    private void Cleanup()
     {
         var self = PilotDefinition.Self;
-        self.MultiVehicleScriptVx = 0;
+        if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
-        self.MultiVehicleScriptVy = 0;
+            !self.TryGetFleetCenterFromSlam(out x, out y, out th))
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
-        self.MultiVehicleAutoHasIdeal = false;
-        self.MultiVehicleAutoEnabled = false;
-    }
-
-    public void Stop()
-    {
-        _stopping = true;
-        Cleanup();
-    }
-
-    private static float Clamp(float value, float min, float max)
-    {
-        if (value < min) return min;
-        if (value > max) return max;
-        return value;
-    }
-
-    private static float ClampAbs(float value, float limit)
-    {
-        var absLimit = Math.Abs(limit);
-        if (absLimit <= 0) return value;
-        if (value > absLimit) return absLimit;
-        if (value < -absLimit) return -absLimit;
-        return value;
-    }
-
-    private static float Slew(float current, float target, float maxDelta)
-    {
-        if (maxDelta <= 0) return target;
-        if (target > current + maxDelta) return current + maxDelta;
-        if (target < current - maxDelta) return current - maxDelta;
-        return target;
-    }
-
-    private static float AverageAngle(float frontTh, float rearTh)
-    {
-        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
-        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
-    }
-
-    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
-        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
-    {
-        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
-        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
-        driveSpeed = speed;
-        frontTh = rawFrontTh;
-        rearTh = rawRearTh;
-        reverseEquivalent = false;
-
-        if (rawBaseTh > limit)
         {
-            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
-            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
-            driveSpeed = -driveSpeed;
+            return;
-            reverseEquivalent = true;
-        }
-        else if (rawBaseTh < -limit)
-        {
-            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
-            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
-            driveSpeed = -driveSpeed;
-            reverseEquivalent = true;
         }
 
-        frontTh = ClampAbs(frontTh, limit);
+        if (!FleetCurveWalk.TryParsePointList(PilotDefinition.Conf.FleetCurveControlPoints,
-        rearTh = ClampAbs(rearTh, limit);
+                out var relativePoints, out var error))
+        {
+            DLog.Log($"FleetCurveWalkTest abort: {error}", "FleetCurveDbg");
+            Hedingben.ToastText(error, "FleetCurve");
+            return;
+        }
+
+        var controlPoints = FleetCurveWalk.BuildRelativeControlPoints(new Vector2(x, y), th, relativePoints);
+        _proc = new FleetCurveWalk
+        {
+            ControlPoints = controlPoints,
+            CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
+            BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
+            SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+            FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+            FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+            SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+            StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
     }
 
-    private static float ProbeSpeed(float speed)
+    public override void TestStop()
     {
-        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+        _proc?.Stop();
-    }
+        _task?.Stop();
-
-    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
-        out float yawSign)
-    {
-        yawSign = 0f;
-        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
-            return false;
-
-        var radius = Math.Max(1f, Math.Abs(controlRadius));
-        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
-            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
-            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
-        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
-        if (!intersect)
-            return false;
-
-        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
-        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
-        var tangentSign = center.Y > 1f ? 1f : -1f;
-        var speedSign = driveSpeed >= 0f ? 1f : -1f;
-        yawSign = speedSign * tangentSign;
-        return true;
-    }
-
-    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
-    {
-        const float probeDth = 1f;
-        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
-            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
-        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
-            ? yawSign
-            : 1f;
-    }
-
-    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
-        Vector2 pathLeft)
-    {
-        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
-        var rad = motionTh / 180f * Math.PI;
-        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
-        if (driveSpeed < 0f)
-            dir = -dir;
-        return Vector2.Dot(dir, pathLeft);
-    }
-
-    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
-        float lateral, float biasProbe)
-    {
-        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
-            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
-        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
-        return -Math.Sign(lateral) * lateralVelocity;
-    }
-
-    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
-        float lateral)
-    {
-        if (Math.Abs(lateral) <= 1e-3f)
-            return 1f;
-
-        const float probeBias = 1f;
-        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
-        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
-        return positiveScore >= negativeScore ? 1f : -1f;
-    }
-
-    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
-        out float centerTh, out string source)
-    {
-        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
-        {
-            source = "fleet";
-            return true;
-        }
-
-        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
-        {
-            source = "slam";
-            return true;
-        }
-
-        source = "none";
-        return false;
-    }
-
-    public override IEnumerable<bool> Get()
-    {
-        var self = PilotDefinition.Self;
-        var conf = PilotDefinition.Conf;
-        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
-        if (chassis == null)
-        {
-            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
-            yield break;
-        }
-        _stopping = false;
-
-        DLog.Log(
-            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
-            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
-            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
-            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
-            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
-            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
-            "FleetCrabDbg");
-
-        if (conf.MultiVehicleMasterEndpoint != "/")
-        {
-            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
-            yield break;
-        }
-
-        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
-        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
-        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
-        {
-            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
-            yield break;
-        }
-        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
-        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
-        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=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} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
-            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
-            $"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;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleAutoEnabled = true;
-        self.MultiVehicleAutoVx = 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 startup sync seqBase={warmSeqBaseline} " +
-            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
-            "FleetCrabDbg");
-
-        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
-        var warmIter = 0;
-        var warmReady = false;
-        var warmDetail = "";
-        while (!_stopping && DateTime.Now < warmEnd)
-        {
-            warmIter++;
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = 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();
-            var snap = self.GetFleetCenterSnapshot();
-            int cnt;
-            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
-            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} " +
-                    $"detail={warmDetail}",
-                    "FleetCrabDbg");
-            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} detail={warmDetail}",
-                    "FleetCrabDbg");
-                break;
-            }
-            yield return true;
-        }
-        if (!warmReady)
-        {
-            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 && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
-        {
-            x0 = warmX;
-            y0 = warmY;
-            theta = warmTh;
-            pathStart = new Vector2(x0, y0);
-            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
-            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
-            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
-            phiRad = phi / 180.0 * Math.PI;
-            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
-            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
-            self.MultiVehicleAutoIdealX = pathStart.X;
-            self.MultiVehicleAutoIdealY = pathStart.Y;
-            self.MultiVehicleAutoIdealTh = targetBodyTh;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-            DLog.Log(
-                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
-                "FleetCrabDbg");
-        }
-
-        var iter = 0;
-        var lastLog = DateTime.MinValue;
-        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
-        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
-        var baseSpeed = Math.Abs(CrabSpeed);
-        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
-        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
-        var accel = Math.Abs(FleetCrabAccel);
-        var startAccel = Math.Abs(FleetCrabStartAccel);
-        var cmdSpeed = 0f;
-        var lastTick = DateTime.Now;
-        var speedRampStart = DateTime.Now;
-        var stopReason = "done";
-
-        while (!_stopping)
-        {
-            iter++;
-
-            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
-            {
-                stopReason = "fleet center invalid";
-                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
-                break;
-            }
-            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
-            var along = Vector2.Dot(delta, pathDir);
-            var lateral = Vector2.Dot(delta, pathLeft);
-            var remain = pathLengthMm - along;
-            if (remain <= finishDistance)
-                break;
-
-            var targetSpeed = baseSpeed;
-            var slowRatio = 1f;
-            if (remain < slowDistance)
-            {
-                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
-                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
-            }
-            var now = DateTime.Now;
-            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
-            lastTick = now;
-            var rampElapsed = (now - speedRampStart).TotalSeconds;
-            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
-            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
-            cmdSpeed = speed;
-
-            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
-            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
-            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
-            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
-            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
-                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
-            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
-            var rawBiasItem = rawBiasMagnitude * biasSign;
-            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
-            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
-            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
-            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
-            var rawFrontTh = baseCrabTh + biasItem + dthItem;
-            var rawRearTh = baseCrabTh + biasItem - dthItem;
-            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
-                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
-            holdFrontTh = frontTh;
-            holdRearTh = rearTh;
-            var idealAlong = Clamp(along, 0f, pathLengthMm);
-            var ideal = pathStart + pathDir * idealAlong;
-
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleScriptVx = 0;
-            self.MultiVehicleScriptVy = 0;
-            self.MultiVehicleScriptVth = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = driveSpeed;
-            self.MultiVehicleAutoFrontTh = frontTh;
-            self.MultiVehicleAutoRearTh = rearTh;
-            self.MultiVehicleAutoIdealX = ideal.X;
-            self.MultiVehicleAutoIdealY = ideal.Y;
-            self.MultiVehicleAutoIdealTh = targetBodyTh;
-            self.MultiVehicleAutoHasIdeal = true;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-
-            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
-            {
-                lastLog = DateTime.Now;
-                var snap = self.GetFleetCenterSnapshot();
-                int fleetCnt;
-                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
-                DLog.Log(
-                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
-                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
-                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
-                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
-                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
-                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
-                    "FleetCrabDbg");
-                DLog.Log(
-                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
-                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
-                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
-                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
-                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
-                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
-                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
-                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
-                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
-                    "FleetCrabHeadingDbg");
-            }
-            yield return true;
-        }
-
-        if (_stopping)
-            stopReason = "stop";
-
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = holdFrontTh;
-        self.MultiVehicleAutoRearTh = holdRearTh;
-        self.MultiVehicleAutoCmdTime = DateTime.Now;
-        DLog.Log(
-            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
-            "FleetCrabDbg");
-        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
-        while (!_stopping && DateTime.Now < settleEnd)
-        {
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = holdFrontTh;
-            self.MultiVehicleAutoRearTh = holdRearTh;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-            yield return true;
-        }
-
-        Cleanup();
-        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
-        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
     }
 }
 

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -203,6 +203,31 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
     public float FleetCrabStartWheelAlignDeg = 2f;
 
+    // ===== Fleet linked Bezier curve walk =====
+    [FieldMember(desc = "FleetCurve MovementTest control points relative to fleet start; format x,y;x,y;...")]
+    public string FleetCurveControlPoints = "0,0;1000,800;2000,800;3000,0";
+
+    [FieldMember(desc = "FleetCurve Bezier resolution")]
+    public int FleetCurveBezierResolution = 100;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [FieldMember(desc = "FleetCurve car direction bias(deg); body target follows tangent+bias")]
+    public float FleetCurveCarDirectionBias = 0f;
+
+    [FieldMember(desc = "FleetCurve slow distance(mm)")]
+    public float FleetCurveSlowDistance = 2000f;
+
+    [FieldMember(desc = "FleetCurve finish distance(mm)")]
+    public float FleetCurveFinishDistance = 20f;
+
+    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+    public float FleetCurveFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "FleetCurve slowing curve exponent")]
+    public float FleetCurveSlowingPow = 0.8f;
+
     // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";