Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

MultiWheel/MultiWheelC/AGV.cs

@@ -1,7 +1,11 @@
 using ClumsyCore;
 using ClumsyCore.Interfaces;
+using ClumsyCore.Sensors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
+using MDCSToolBox.Clumsy.Calibration;
 using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Tracks;
 using MDCSToolBox.Commons.Controllers;
@@ -10,6 +14,7 @@ using System;
 using System.Collections.Generic;
 using System.Net.Http;
 using System.Numerics;
+using System.Security.Cryptography;
 using System.Threading;
 using System.Threading.Tasks;
 using static ClumsyCore.DTools.Painter;
@@ -64,6 +69,25 @@ namespace MultiWheelC
                 PilotDefinition.Self.IOObstacleArea = area;
             }
         }
+        public void RotateToTarget(float target)
+        {
+            //if (!needrotate) return;
+            var dl = new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = target,
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get());
+            dl.Wait();
+        }
 
         //参数1：tireNum 需要钻过的轮胎对数量
         //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
@@ -152,7 +176,7 @@ namespace MultiWheelC
         }
 
         //离车一定是后雷达识别一个轮胎
-        public void LeaveCar(int srcId, int dstId)
+        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
         {
             while (!TryLock(dstId))
             {
@@ -160,7 +184,8 @@ namespace MultiWheelC
             }
             DLog.Log($"锁点{dstId}完成", "TireFollowing");
             DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
-
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
             var following = new TireFollowing()
             {
                 GetController = () => new ChassisController().Get(),
@@ -173,7 +198,7 @@ namespace MultiWheelC
                         DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
                         StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
                         StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
                         FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                         PathTransformation = new Tuple<float, float, float>(
                         PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
@@ -186,13 +211,91 @@ namespace MultiWheelC
                 },
                 CarDirection = 180f,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                MaxSpeed = 0.25f,
+                EnableHandover = true,
+                HandoverDistance = 200f,
+                HandoverSpeed = 0.3f,
                 WalkBlindTh = 0,
                 TireNum = 1
             };
-            var _dt = new DriveTask(following.Get());
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    CarDirectionBias = 180f,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            var _dt = new DriveTask(LeaveThenFollow());
             _dt.Wait();
-            DLog.Log("钻车动作结束", "TireFollowing");
+            DLog.Log("离车动作1结束", "TireFollowing");
+        }
+
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    EnableHandover = true,
+                    HandoverDistance = 200, 
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
+                while (!TryLock(426))
+                {
+                    Thread.Sleep(20);
+                }
+                Leave(dstId);
+                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            var _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作2结束", "TireFollowing");
         }
 
         //驱动器上使能
@@ -226,18 +329,13 @@ namespace MultiWheelC
                 RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
             }.Get()).Wait();
         }
-        // 车队联动-自动蟹行：供调度按绝对起终点触发。
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
-        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
         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)
             {
@@ -245,6 +343,26 @@ 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);
+            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
+            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
+            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}, " +
+                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
+                "FleetCrabDbg");
+
             if (dstId != -1)
             {
                 while (!TryLock(dstId))
@@ -256,15 +374,12 @@ namespace MultiWheelC
 
             var action = new MultiWheelC.FleetCrabWalk
             {
-                UseAbsolutePath = true,
+                CrabAngleDeg = crabAngle,
-                PathStartX = srcX,
+                BodyToPathAngleDeg = bodyToPathAngle,
-                PathStartY = srcY,
-                PathEndX = dstX,
-                PathEndY = dstY,
                 CrabLengthMm = pathLength,
                 CrabSpeed = speed,
-                BodyToPathAngleDeg = -CarDirectionBias,
                 FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
                 FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
                 FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
                 FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
@@ -285,19 +400,100 @@ namespace MultiWheelC
                 }
             }
         }
-        public void LineTracking(int srcId, int dstId)
+
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
         {
-            while (!TryLock(dstId))
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
             {
-                Thread.Sleep(50);
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
             }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+
-            new DriveTask(new LineTracking()
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
             {
-                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
-                LeaveSrcFunction = Leave,
+                    "FleetCurveDbg");
-                SrcId = srcId,
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
-            }.Get()).Wait();
+                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 = 0f;
+
+            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=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 = 0f,
+                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($"release srcId={srcId}", "FleetCurveDbg");
+                }
+            }
         }
         
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)

MultiWheel/MultiWheelC/FleetCrabWalk.cs

@@ -0,0 +1,526 @@
+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.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,411 @@
+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.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.TireFollowing.cs

@@ -292,6 +292,206 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public class DstTrackerForward : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public class DstTrackerhoutui : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 180f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先直行再终点跟踪")]
+    public class LineTrackThenDstTrackerTest : MovementTest
+    {
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    EnableHandover = true,
+                    HandoverDistance = 200f,
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = carDirectionBias,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先离车再终点跟踪")]
+    public class LeaveCarThenDstTrackerTest : MovementTest
+    {
+        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                var following = new TireFollowing()
+                {
+                    GetController = () => new ChassisController().Get(),
+                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                    detectors = new List<TireFollowing.DetectorDefinition>()
+                    {
+                        new TireFollowing.DetectorDefinition()
+                        {
+                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                            StartGuessingY = 0,
+                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                            PathTransformation = new Tuple<float, float, float>(
+                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                                0),
+                        },
+                    },
+                    CarDirection = 180f,
+                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                    WalkBlindTh = 0,
+                    TireNum = 1
+                };
+
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+
     [MovementTest(name = "驱动器下使能测试")]
     public class DriverDisableTest : MovementTest
     {
@@ -320,6 +520,35 @@ namespace MultiWheelC
         }
     }
 
+    [MovementTest(name = "底盘旋转测试")]
+    public class RotateToAngleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            var target = UI.GetInput("输入旋转角度：");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = float.Parse(target),
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get()).Wait();
+        }
+    }
+
     public class utils
     {
         public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)

MultiWheel/MultiWheelC/MovementTests.cs

@@ -7,7 +7,6 @@ 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;
@@ -426,351 +425,57 @@ 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>使用调度给定的绝对起终点路径，而不是用当前车队中心和 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;
-
-    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
-    public float FleetCrabAccel = 0.2f;
-
-    /// <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;
+            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
-        self.MultiVehicleScriptEnabled = false;
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
-        self.MultiVehicleAutoVx = 0;
+            return;
-        self.MultiVehicleAutoFrontTh = 0;
+        }
-        self.MultiVehicleAutoRearTh = 0;
+
-        self.MultiVehicleAutoHasIdeal = false;
+        var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
-        self.MultiVehicleAutoEnabled = false;
+        var controlPoints = new List<Vector2>();
+        for (var i = 0; i < pointCount; i++)
+            controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
+        var fleetCenter = new Vector2(x, y);
+        if (Vector2.Distance(fleetCenter, controlPoints[0]) >
+            Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
+            controlPoints.Reverse();
+        var track = new BezierTrack(controlPoints)
+        {
+            Speed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f
+        };
+
+        _proc = new FleetCurveWalk
+        {
+            Track = track,
+            CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f,
+            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();
     }
 
-    public void Stop()
+    public override void TestStop()
     {
-        _stopping = true;
+        _proc?.Stop();
-        Cleanup();
+        _task?.Stop();
-    }
-
-    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;
-    }
-
-    public override IEnumerable<bool> Get()
-    {
-        var self = PilotDefinition.Self;
-        var conf = PilotDefinition.Conf;
-        _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={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " +
-            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
-            $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac: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 (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
-        {
-            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
-            yield break;
-        }
-        DLog.Log($"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 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")} " +
-            $"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} " +
-            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
-            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
-            "FleetCrabDbg");
-
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleAutoEnabled = true;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
-        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");
-
-        var warmEnd = DateTime.Now.AddSeconds(2.0);
-        var warmIter = 0;
-        var warmReady = false;
-        while (!_stopping && DateTime.Now < warmEnd)
-        {
-            warmIter++;
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = 0;
-            self.MultiVehicleAutoRearTh = 0;
-            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}",
-                    "FleetCrabDbg");
-            if (cnt >= conf.MultiVehicleFleetNum)
-            {
-                warmReady = true;
-                DLog.Log(
-                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
-                    "FleetCrabDbg");
-                break;
-            }
-            yield return true;
-        }
-        if (!warmReady)
-            DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
-                "FleetCrabDbg");
-
-        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
-
-        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 cmdSpeed = 0f;
-        var lastTick = DateTime.Now;
-        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
-        var holdRearTh = holdFrontTh;
-        var stopReason = "done";
-
-        while (!_stopping)
-        {
-            iter++;
-
-            if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
-            {
-                stopReason = "fleet center invalid";
-                DLog.Log("ABORT: TryGetFleetCenterFromSlam 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 speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed;
-            cmdSpeed = speed;
-
-            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
-            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
-            var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
-            var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
-            biasItem = ClampAbs(biasItem, conf.BiasThreshold);
-            var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
-            var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
-            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 = speed;
-            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} 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} auto=(vx:{speed: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");
-            }
-            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");
     }
 }
 
@@ -789,6 +494,7 @@ public class FleetCrabWalkTest : MovementTest
             CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
             CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
             FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+            FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
             FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
             FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
             FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,

MultiWheel/MultiWheelC/Movements.cs

@@ -25,6 +25,7 @@ using System.Numerics;
 using System.Reflection;
 using System.Text;
 using System.Threading;
+using static ClumsyCore.DTools.Painter;
 
 namespace MultiWheelC
 {
@@ -167,6 +168,41 @@ namespace MultiWheelC
         }
     }
 
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+
+        public float InitialSendSpeed = 0;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+            Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+
+           if (InitialSendSpeed != 0)
+            {
+                tracker.SkipInitialRotate = true;
+                tracker.InitialSendSpeed = InitialSendSpeed;
+            }
+
+            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
+            tracker.AddTrack(linePath);
+            var task = new DriveTask(tracker.Track());
+            task.Wait();
+
+            // 到点后兜底停车
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SendXYThSpeed(0f, 0f, 0f);
+            yield return false;
+        }
+    }
+
     //直线行走基于轮里程
     public class LineTracking : MovementDefinition
     {
@@ -180,44 +216,40 @@ namespace MultiWheelC
         public int DstId = -1;
         public Action<int> LeaveSrcFunction = null;
         private PIDController pid;
-
+        // 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
-        // GhostMode 虚拟里程计
+        public bool EnableHandover = false;
-        private float _ghostDistance;
+        public float HandoverDistance = 80f; // mm
-        private DateTime _lastTick;
+        public float HandoverSpeed = 0.15f;  // m/s
 
         public override IEnumerable<bool> Get()
         {
-            bool isGhost = PilotDefinition.Self.GhostMode;
-            if (isGhost)
-            {
-                _ghostDistance = 0f;
-                _lastTick = DateTime.Now;
-            }
 
             pid = new PIDController(() =>
-                isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
                 Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
             { SpeedAccPerSec = MaxSpeed / 2f };
 
             var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            //chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
             while (true)
             {
+                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                var remain = Target - current;
+                if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
+                {
+                    var handoverSign = Math.Sign(remain);
+                    if (handoverSign == 0) handoverSign = 1;
+                    var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
+                    Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
+                    chassis.SendXYThSpeed(handoverSpeed, 0, 0);
+                    // 保留一拍接力速度，让后续 DstTracker 无缝接管
+                    yield return true;
+                    break;
+                }
                 var speed = pid.GetResponse(Target);
+                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
                 
-                if (isGhost)
-                {
-                    var now = DateTime.Now;
-                    float dt = (float)(now - _lastTick).TotalSeconds;
-                    _ghostDistance += speed * dt * 1000f;
-                    _lastTick = now;
-                    Console.WriteLine($"[Ghost] output: {speed:F2}  current: {_ghostDistance:F2}");
-                }
-                else
-                {
-                    Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
-                }
-
-                var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
                 chassis.SendXYThSpeed(speed, 0, 0);
 
                 if (pid.IsArrived()) break;
@@ -234,26 +266,60 @@ namespace MultiWheelC
 
     public class DriverAble : MovementDefinition
     {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器上使能");
             PilotDefinition.Self.ResetFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.ResetFromC = false;
-            Console.WriteLine("驱动器上使能完成");
+            if (success)
+                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }
 
     public class DriverDisable : MovementDefinition
     {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器下使能");
             PilotDefinition.Self.DisableFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = !PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = !PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.DisableFromC = false;
-            Console.WriteLine("驱动器下使能完成");
+            if (success)
+                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -6,15 +6,14 @@ namespace MultiWheelC;
 
 public class PilotConfig : MultiWheelPilotConfig
 {
-    // ===== 多车联动（已从 MDCSToolBox 内联回 Tutorial）=====
+    [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
-    [FieldMember(desc = "[sync] 转向角加速度(deg/s^2)")] public float SyncThAccPerSec = 30f;
+    [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
-    [FieldMember(desc = "[sync] 编队车间距(mm)")] public float TestCarSyncDistance = 2400f;
+    [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
-    [FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f;
+    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
-    // 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg)，此处系数保持 1（直通），不要再次缩放。
+    [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
-    [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
+    [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
-    // Manual crab mode: VxFac scales linear speed, VyFac maps steer stick ratio to crab steer angle in degrees.
+    [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
-    [FieldMember(desc = "[sync] 手动Vy系数(蟹行满杆舵角deg)")] public float ManualCarSyncVyFac = 60f;
+    [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
-    [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
     [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
     [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
     // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
@@ -46,9 +45,6 @@ public class PilotConfig : MultiWheelPilotConfig
 
     [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
 
-    // E: 编队控制点半径(mm)。0 表示自动取 syncDistance/2（与 SetOriginBias 几何一致），>0 时按本值固定。
-    //    取代历史硬编码 510，避免改间距后控制点半径不跟随导致转向/补偿几何错位。
-    [FieldMember(desc = "多车联动：控制点半径(mm，0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f;
     // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
     //    即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
     [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
@@ -161,6 +157,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
     public float FleetCrabAngleDeg = 45f;
 
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
     [FieldMember(desc = "车队蟹行：路径长度(mm)")]
     public float FleetCrabLengthMm = 2000f;
 
@@ -170,6 +169,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
     public float FleetCrabAccel = 0.2f;
 
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
     [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
     public float FleetCrabSlowDistance = 2000f;
 
@@ -185,6 +187,37 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
     public float FleetCrabGcpThetaThreshold = 95f;
 
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
+    public float FleetCrabDthLinearFac = 1f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
+    public float FleetCrabDthLinearThreshold = 10f;
+
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
+    // ===== Fleet linked Bezier curve walk =====
+    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
+    public int FleetCurveTestControlPointCount = 4;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [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";
@@ -287,5 +320,15 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
     [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
 
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
+    [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
     #endregion
+
+    [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
+    [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
+    [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
+    [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
+    [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
 }

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -37,14 +37,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode;
     [FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx;
+    [AsLowerIO(desc = "多车联动：遥控器Vx比例")] public float MultiVehicleManualVx;
-    [AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy;
+    [AsLowerIO(desc = "多车联动：遥控器Vy比例")] public float MultiVehicleManualVy;
-    [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth;
+    [AsLowerIO(desc = "多车联动：遥控器Vth比例")] public float MultiVehicleManualVth;
+    [AsLowerIO(desc = "多车联动：遥控暂停")] public bool MultiVehicleHold;
 
     // ===== Clumsy 侧脚本/动作驱动的手动等价输入（不走 Medulla IO，不会被 IO 同步覆盖）=====
     // 手动 IO 字段是 [AsLowerIO]（Medulla→Clumsy，Medulla 每周期回写），Clumsy 侧 MovementTest 写它们会被覆盖。
-    // 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动：
+    // 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动。
-    // ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转；Vx/Vy/Vth 语义与手动遥控完全一致（m/s、m/s、deg/s）。
+    // ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转。
+    // 注意：Medulla 遥控 IO 是归一化摇杆比例；脚本字段保持物理量，避免动作参数再被 FleetManual* 二次缩放。
+    // Mode0: Vx=m/s, Vth=目标舵角deg；Mode1: Vx=m/s, Vy=蟹行舵角deg；Mode2: Vth=角速度deg/s。
     [FieldMember(desc = "多车联动：脚本驱动使能")] public bool MultiVehicleScriptEnabled;
     [FieldMember(desc = "多车联动：脚本驱动模式")] public int MultiVehicleScriptMode;
     [FieldMember(desc = "多车联动：脚本驱动Vx")] public float MultiVehicleScriptVx;
@@ -132,6 +135,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
+    [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
 
     private float _multiVehicleAccumulateTh;
     private DateTime _multiVehicleLastThTime = DateTime.Now;
@@ -213,7 +217,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
         DLog.Log(
             $"INPUT master={isMaster} endpoint={Conf.MultiVehicleMasterEndpoint} selfEndpoint={Conf.MultiVehicleSelfEndpoint} car={CarNum} " +
-            $"rawEn={MultiVehicleManualEnabled} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
+            $"rawEn={MultiVehicleManualEnabled} rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
             $"scriptOn={scriptOn} effEn={manualEnabled} effMode={manualMode} effVx={manualVx:0.000} effVy={manualVy:0.000} effVth={manualVth:0.000} " +
             $"auto={autoEnabled} notifFresh={notifFresh} notifAgeMs={notifAgeMs:0} fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum}",
             "MultiVehicleRemoteDbg");
@@ -279,10 +283,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         if (!rotateActive)
         {
             _multiVehicleRotateModeActive = false;
-            MultiVehicleRotateWheelsReady = true;
+            if (!active)
+            {
+                MultiVehicleRotateWheelsReady = true;
+                _multiVehicleRotateAlignDetail = "";
+            }
             MultiVehicleRotateFleetReady = true;
             _multiVehicleRotateDirectionHint = 1f;
-            _multiVehicleRotateAlignDetail = "";
             return;
         }
 
@@ -620,6 +627,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
         var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
         var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
+        var manualHold = !scriptOn && MultiVehicleHold;
         VehicleSyncNotification activeNotification = null;
 
         if (isMaster)
@@ -656,7 +664,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                              Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
             FleetDiag(
                 $"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
-                $"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
+                $"Hold={MultiVehicleHold} Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
                 $"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
                 $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
                 $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
@@ -677,7 +685,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             ResetMultiVehicleRotateCenterDrift();
             LogMultiVehicleRemoteDecision(
                 $"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
-                $"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
+                $"rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
             UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
             sendMotionPainter.Clear();
             lock (FleetLock)
@@ -758,10 +766,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             {
                 syncTh = 0;
                 fleetMode = manualMode;
+                var remoteRatioInput = !scriptOn;
                 if (fleetMode == 2)
                 {
-                    // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
+                    // 外部遥控: Vth 为摇杆比例；内部脚本: Vth 为实际角速度(deg/s)。
-                    fleetOmega = manualVth;
+                    fleetOmega = remoteRatioInput
+                        ? ClampFloat(manualVth, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxRotateOmegaDegPerSec)
+                        : manualVth;
                     fleetVx = 0;
                     fleetFrontTh = 0;
                     fleetRearTh = 0;
@@ -770,12 +781,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else if (fleetMode == 1)
                 {
-                    // 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
+                    // 外部遥控: Vx/Vy 为摇杆比例；内部脚本: Vx 为线速度(m/s), Vy 为蟹行舵角(deg)。
-                    var speed = manualVx * Conf.ManualCarSyncVxFac;
+                    var speed = remoteRatioInput
-                    var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
+                        ? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
-                    var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
+                        : manualVx;
+                    var crabRatio = remoteRatioInput ? ClampFloat(manualVy, -1f, 1f) : 0f;
+                    var crabAngle = remoteRatioInput
+                        ? crabRatio * Math.Abs(Conf.FleetManualMaxCrabAngleDeg)
+                        : manualVy;
                     crabInputVx = speed;
-                    crabInputVy = crabRatio;
+                    crabInputVy = remoteRatioInput ? crabRatio : crabAngle;
                     crabRawAngle = crabAngle;
                     crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
 
@@ -789,8 +804,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else
                 {
-                    fleetVx = manualVx * Conf.ManualCarSyncVxFac;
+                    // 外部遥控: Vx/Vth 为摇杆比例；内部脚本: Vx 为线速度(m/s), Vth 为目标舵角(deg)。
-                    var targetTh = manualVth * Conf.ManualCarSyncVthFac;
+                    fleetVx = remoteRatioInput
+                        ? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
+                        : manualVx;
+                    var targetTh = remoteRatioInput
+                        ? ClampFloat(manualVth, -1f, 1f) * Math.Abs(Conf.FleetManualMaxSteerAngleDeg)
+                        : manualVth;
                     var now = DateTime.Now;
                     var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
                     _multiVehicleLastThTime = now;
@@ -800,6 +820,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     fleetFrontTh = _multiVehicleAccumulateTh;
                     fleetRearTh = -fleetFrontTh;
                 }
+
+                if (manualHold)
+                {
+                    fleetMode = 0;
+                    fleetVx = 0;
+                    fleetFrontTh = 0;
+                    fleetRearTh = 0;
+                    fleetOmega = 0;
+                    _multiVehicleAccumulateTh = 0f;
+                    _multiVehicleLastThTime = DateTime.Now;
+                }
             }
             else
             {
@@ -826,11 +857,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
             }
         }
-        else if (MultiVehicleNotification != null)
+        else if (activeNotification != null)
         {
-            VehicleSyncNotification notification;
+            var notification = activeNotification;
-            lock (_multiVehicleNotificationLock)
-                notification = MultiVehicleNotification;
 
             syncTh = notification.SyncTh;
             syncDistance = notification.SyncDistance;
@@ -864,6 +893,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
 
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
+        var inferredFleetCenterValid = false;
+        float inferredFleetCenterX = 0, inferredFleetCenterY = 0, inferredFleetCenterTh = 0;
+        Tuple<float, float, float> supposedPosForDiag = null;
 
         if (isMaster)
         {
@@ -876,7 +908,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             }
 
             if (TryInferFleetCenter(out var cx, out var cy, out var cth))
+            {
+                inferredFleetCenterValid = true;
+                inferredFleetCenterX = cx;
+                inferredFleetCenterY = cy;
+                inferredFleetCenterTh = cth;
                 PublishFleetCenter(cx, cy, cth);
+            }
 
             // D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
             //    使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
@@ -1085,10 +1123,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         if (fleetReady && !fleetStopActive)
         {
             chassis.SetOriginBias(layoutX, layoutY, layoutTh);
-            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
+            // E: 统一控制点半径，取 syncDistance/2，与编队几何一致。
-            var controlRadius = Conf.MultiVehicleControlRadius > 0
+            var controlRadius = syncDistance / 2f;
-                ? Conf.MultiVehicleControlRadius
-                : syncDistance / 2f;
             chassis.ControlPointRadius = controlRadius;
             // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
             chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;
@@ -1114,6 +1150,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 var supposedPos = LessMath.Transform2D(
                     Tuple.Create(CenterX, CenterY, CenterTh),
                     Tuple.Create(layoutX, layoutY, layoutTh));
+                supposedPosForDiag = supposedPos;
                 var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
                 posBiasX = (float)posBias.Item1;
                 posBiasY = (float)posBias.Item2;
@@ -1298,10 +1335,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _mvRotCompVx = 0;
                 _mvRotCompVy = 0;
                 _mvRotCompOmega = 0;
-                MultiVehicleRotateWheelsReady = true;
                 MultiVehicleRotateFleetReady = true;
-                _multiVehicleRotateAlignDetail = "";
+                // Reset rotate-only PI state outside rotate mode.
-                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
                 _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                 _rotPiLastTime = DateTime.MinValue;
                 _rotPoseEpisode = false;
@@ -1312,6 +1347,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     localCompensateX: xDetectCompensate + xPosCompensate,
                     localCompensateY: yDetectCompensate + yPosCompensate,
                     localCompensateTh: thDetectCompensate + thPosCompensate);
+                if (motionOk)
+                    MultiVehicleRotateWheelsReady = TryCheckRotateWheelAlignment(chassis,
+                        Math.Max(0.1f, Conf.FleetCrabStartWheelAlignDeg), out _multiVehicleRotateAlignDetail);
+                else
+                {
+                    MultiVehicleRotateWheelsReady = false;
+                    _multiVehicleRotateAlignDetail = chassis.LastMotionDecomposeFailureReason;
+                }
+                MultiVehicleRotateFleetReady = true;
                 SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
                 if (!motionOk)
                 {
@@ -1324,6 +1368,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                     $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                     $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
                     $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
+                    $"wheelReady={MultiVehicleRotateWheelsReady} align={_multiVehicleRotateAlignDetail} " +
                     $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
             }
         }
@@ -1339,6 +1384,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             var cx = xDetectCompensate + xPosCompensate;
             var cy = yDetectCompensate + yPosCompensate;
             var cth = thDetectCompensate + thPosCompensate;
+            var actualCenterThForDiag = inferredFleetCenterValid ? inferredFleetCenterTh : CenterTh;
+            var idealHeadingErr = (float)CommonMath.ThDiff(MultiVehicleAutoIdealTh, actualCenterThForDiag);
+            var idealHeadingErrReverse = (float)CommonMath.ThDiff(actualCenterThForDiag, MultiVehicleAutoIdealTh);
+            var frontRearDiff = (float)CommonMath.ThDiff(fleetFrontTh, fleetRearTh);
+            var commandHeadingSplit = frontRearDiff / 2f;
+            var commandBaseTh = (float)CommonMath.RoundTh(fleetRearTh + commandHeadingSplit);
+            var supposedPosText = supposedPosForDiag == null
+                ? "N/A"
+                : $"({supposedPosForDiag.Item1:0},{supposedPosForDiag.Item2:0},{supposedPosForDiag.Item3:0.0})";
             var crabDbg = isMaster && manualEnabled && fleetMode == 1
                 ? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
                 : "";
@@ -1366,6 +1420,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             DLog.Log(dbg, "MultiVehicleDbg");
             FleetDiag(dbg);
 
+            if (autoMode || autoEnabled || MultiVehicleAutoEnabled)
+                DLog.Log(
+                    $"APPLY car{CarNum} master:{isMaster} autoMode:{autoMode} manual:{manualEnabled} " +
+                    $"cmd(vx:{fleetVx:0.000},f:{fleetFrontTh:0.00},r:{fleetRearTh:0.00},base:{commandBaseTh:0.00},split:{commandHeadingSplit:0.00},f-r:{frontRearDiff:0.00}) " +
+                    $"ideal(has:{MultiVehicleAutoHasIdeal},x:{MultiVehicleAutoIdealX:0},y:{MultiVehicleAutoIdealY:0},th:{MultiVehicleAutoIdealTh:0.00}) " +
+                    $"centerPublished({CenterX:0},{CenterY:0},{CenterTh:0.00}) " +
+                    $"centerInferred(valid:{inferredFleetCenterValid},x:{inferredFleetCenterX:0},y:{inferredFleetCenterY:0},th:{inferredFleetCenterTh:0.00}) " +
+                    $"idealHeadingErr(target-actual):{idealHeadingErr:0.00} reverse(actual-target):{idealHeadingErrReverse:0.00} " +
+                    $"layout({layoutX:0},{layoutY:0},{layoutTh:0.00}) self({selfX:0},{selfY:0},{selfTh:0.00}) supposed:{supposedPosText} " +
+                    $"detectDth:{detectDth:0.00}->comp:{thDetectCompensate:0.00} " +
+                    $"posBiasTh:{posBiasTh:0.00}->comp:{thPosCompensate:0.00} " +
+                    $"totalLocalComp(x:{cx:0.0},y:{cy:0.0},th:{cth:0.00}) " +
+                    $"useDetourCorr:{useDetourCorrection} slam:{slamRead} fleetPos:{fleetPosValid} ready:{fleetReady}",
+                    "FleetCrabHeadingDbg");
+
             // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
             Hedingben.ToastText(
                 $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
@@ -1520,6 +1589,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return true;
     }
 
+    public bool TryGetFleetCenterFromMembers(out float centerX, out float centerY, out float centerTh)
+    {
+        return TryInferFleetCenter(out centerX, out centerY, out centerTh);
+    }
+
     private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
     {
         // 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。
@@ -1591,15 +1665,64 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return true;
     }
 
-    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
+    public long BeginFleetMotionWarmup()
     {
-        // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
+        MultiVehicleRotateWheelsReady = false;
-        // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
+        MultiVehicleRotateFleetReady = false;
+        _multiVehicleRotateAlignDetail = "fleet motion warmup pending";
+        return Interlocked.Read(ref _multiVehicleNotifySeq);
+    }
+
+    public bool IsFleetMotionWarmupReady(DateTime warmStartTime, long notificationSeqBaseline,
+        float syncTh, float syncDistance, out string detail)
+    {
+        var pending = new List<string>();
+        var requiredCount = Math.Max(1, Conf.MultiVehicleFleetNum);
+        lock (FleetLock)
+        {
+            if (MultiVehicleFleet.Count != requiredCount)
+                pending.Add($"fleetCnt={MultiVehicleFleet.Count}/{requiredCount}");
+
+            foreach (var kv in MultiVehicleFleet.OrderBy(k => k.Key))
+            {
+                var carNum = kv.Key;
+                var info = kv.Value;
+                if (!_multiVehicleFleetSeen.TryGetValue(carNum, out var seen) || seen < warmStartTime)
+                    pending.Add($"car{carNum}:notFresh");
+
+                var (layoutX, layoutY, layoutTh) = GetLayoutPoseForCar(carNum, syncTh, syncDistance);
+                if (Math.Abs(info.LayoutX - layoutX) > 1f ||
+                    Math.Abs(info.LayoutY - layoutY) > 1f ||
+                    Math.Abs(CommonMath.ThDiff(info.LayoutTh, layoutTh)) > 1f)
+                    pending.Add(
+                        $"car{carNum}:layout=({info.LayoutX:0},{info.LayoutY:0},{info.LayoutTh:0.0})");
+
+                if (!info.MotionFeasible)
+                    pending.Add($"car{carNum}:motion={info.MotionInfeasibleReason}");
+                if (!info.RotateWheelsAligned)
+                    pending.Add($"car{carNum}:wheel={info.RotateWheelAlignDetail}");
+                if (carNum != CarNum && info.AppliedNotificationSeq <= notificationSeqBaseline)
+                    pending.Add($"car{carNum}:seq={info.AppliedNotificationSeq}<={notificationSeqBaseline}");
+            }
+        }
+
+        detail = pending.Count == 0
+            ? $"ready seqBase={notificationSeqBaseline}"
+            : string.Join("; ", pending);
+        return pending.Count == 0;
+    }
+
+    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
+        => GetLayoutPoseForCar(CarNum, syncTh, syncDistance);
+
+    private static (float, float, float) GetLayoutPoseForCar(int carNum, float syncTh, float syncDistance)
+    {
+        // Two-car layout: members are mirrored around the fleet center; car2 faces 180 deg away.
         var rad = syncTh / 180f * Math.PI;
-        var sign = CarNum == 1 ? 1f : -1f;
+        var sign = carNum == 1 ? 1f : -1f;
         var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign);
         var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign);
-        return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180));
+        return (xx, yy, syncTh + (carNum == 1 ? 0 : 180));
     }
 
     private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
@@ -1624,7 +1747,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             MotionFeasible = _multiVehicleMotionFeasible,
             MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
             RotateWheelsAligned = MultiVehicleRotateWheelsReady,
-            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail,
+            AppliedNotificationSeq = _multiVehicleAppliedSeq
         };
     }
 

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -50,6 +50,11 @@ namespace MultiWheelC
         /// </summary>
         public float MaxSpeed = 0.3f;
 
+        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 200f; // mm
+        public float HandoverSpeed = 0.2f;    // m/s
+
         /// <summary>
         /// 钻轮胎数量
         /// </summary>
@@ -200,7 +205,12 @@ namespace MultiWheelC
             controller.FinishDistance = float.MinValue;
             controller.FirstThAccuracy = 999;
             _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
-
+            void HardStop()
+            {
+                _dt?.Stop();
+                ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
+                DLog.Log($"Hard Stop!", "TireFollowing");
+            }
             float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
             bool WalkBlindStage1 = false, WalkBlindStage2 = false;
             var angle2target = -1f;
@@ -280,9 +290,16 @@ namespace MultiWheelC
                     //第二次盲走时或只钻一个轮胎时
                     if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
                     {
+                        //controller.FinishDistance = 10f;
                         controller.SlowDistance = SlowDistance;
                         controller.SlowingPow = 0.7f;
                     }
+                    if (EnableHandover)
+                    {
+                        controller.SlowDistance = float.MinValue;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
+                    }
                     (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
@@ -318,16 +335,19 @@ namespace MultiWheelC
                             _remainAngleList.Clear();
                             _remainDistanceList.Clear();
                             detectorIndex++;
-                            if (detectors.Count == 1 || TireNum == 1)
+                            if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
                             {
-                                _dt.Stop();
+                                HardStop();
                                 yield return false;
                             }
                         }
                         else if (WalkBlindStage2)
                         {
                             DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
-                            _dt.Stop();
+                            if (!EnableHandover)
+                            {
+                                HardStop();
+                            }
                             yield return false;
                         }
                     }
@@ -397,6 +417,14 @@ namespace MultiWheelC
                     while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
                     rd = _remainDistanceList.Average();
                     _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
+                    if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
+                    {
+                        if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                        {
+                            detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                        }
+                    }
 
                     if (detectorIndex < detectors.Count - 1)
                     {
@@ -404,11 +432,11 @@ namespace MultiWheelC
                         if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
                         {
                             WalkBlindStage1 = true;
-                            if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                            {
+                            //{
-                                detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                                DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                            }
+                            //}
                             WalkBlindCarPathDstX = trackDst.X;
                             WalkBlindCarPathDstY = trackDst.Y;
                             WalkBlindCarPathDstTh = angle2target + WalkBlindTh;

MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs

@@ -7,7 +7,7 @@ namespace MultiWheelC;
 
 internal static class VehicleSyncBinaryCodec
 {
-    private const byte Version = 1;
+    private const byte Version = 2;
     private const byte RegisterType = 1;
     private const byte NotificationType = 2;
     private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
@@ -27,9 +27,9 @@ internal static class VehicleSyncBinaryCodec
     {
         using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
         using var reader = new BinaryReader(stream, Encoding.UTF8);
-        ReadHeader(reader, RegisterType);
+        var version = ReadHeader(reader, RegisterType);
         var carNum = reader.ReadInt32();
-        var info = ReadInfo(reader);
+        var info = ReadInfo(reader, version);
         EnsureFullyRead(stream);
         return (carNum, info);
     }
@@ -88,7 +88,7 @@ internal static class VehicleSyncBinaryCodec
     {
         using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
         using var reader = new BinaryReader(stream, Encoding.UTF8);
-        ReadHeader(reader, NotificationType);
+        var version = ReadHeader(reader, NotificationType);
 
         var notification = new VehicleSyncNotification
         {
@@ -129,7 +129,7 @@ internal static class VehicleSyncBinaryCodec
         for (var i = 0; i < fleetCount; ++i)
         {
             var carNum = reader.ReadInt32();
-            notification.Fleet[carNum] = ReadInfo(reader);
+            notification.Fleet[carNum] = ReadInfo(reader, version);
         }
 
         EnsureFullyRead(stream);
@@ -144,7 +144,7 @@ internal static class VehicleSyncBinaryCodec
         writer.Write((ushort)0);
     }
 
-    private static void ReadHeader(BinaryReader reader, byte expectedType)
+    private static byte ReadHeader(BinaryReader reader, byte expectedType)
     {
         for (var i = 0; i < Magic.Length; ++i)
         {
@@ -153,7 +153,7 @@ internal static class VehicleSyncBinaryCodec
         }
 
         var version = reader.ReadByte();
-        if (version != Version)
+        if (version < 1 || version > Version)
             throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
 
         var type = reader.ReadByte();
@@ -163,6 +163,8 @@ internal static class VehicleSyncBinaryCodec
         var reserved = reader.ReadUInt16();
         if (reserved != 0)
             throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
+
+        return version;
     }
 
     private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
@@ -178,9 +180,10 @@ internal static class VehicleSyncBinaryCodec
         writer.Write(info.LayoutTh);
         WriteString(writer, info.MotionInfeasibleReason);
         WriteString(writer, info.RotateWheelAlignDetail);
+        writer.Write(info.AppliedNotificationSeq);
     }
 
-    private static VehicleSyncInfo ReadInfo(BinaryReader reader)
+    private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
     {
         var info = new VehicleSyncInfo();
         ApplyInfoFlags(info, reader.ReadUInt16());
@@ -194,6 +197,7 @@ internal static class VehicleSyncBinaryCodec
         info.LayoutTh = reader.ReadSingle();
         info.MotionInfeasibleReason = ReadString(reader);
         info.RotateWheelAlignDetail = ReadString(reader);
+        info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
         return info;
     }
 

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -23,6 +23,7 @@ public class VehicleSyncInfo
     [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
     [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
     [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
+    [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
 }
 
 public class VehicleSyncNotification

MultiWheel/MultiWheelM/CartDefinition.cs

@@ -89,22 +89,25 @@ public partial class CartDefinition : CartActivator.CartDefinition
     [AsLowerIO(desc = "（手动）多车联动模式")]
     public int MultiVehicleManualMode;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vx")]
+    [AsLowerIO(desc = "多车联动：遥控器Vx比例")]
     public float MultiVehicleManualVx;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vy")]
+    [AsLowerIO(desc = "多车联动：遥控器Vy比例")]
     public float MultiVehicleManualVy;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vth")]
+    [AsLowerIO(desc = "多车联动：遥控器Vth比例")]
     public float MultiVehicleManualVth;
 
-    [AsInitParam(desc = "手动最大速度(m/s)")]
+    [AsLowerIO(desc = "多车联动：遥控暂停")]
+    public bool MultiVehicleHold;
+
+    [AsInitParam(desc = "普通手动最大速度(m/s)，车队联动不使用")]
     public float MaxManualSpeed = 0.3f;
 
-    [AsInitParam(desc = "手动最大自旋角速度(deg/s)")]
+    [AsInitParam(desc = "普通手动最大自旋角速度(deg/s)，车队联动不使用")]
     public float MaxManualAngularSpeed = 45f;
 
-    [AsInitParam(desc = "手动最大转向角度(deg)")]
+    [AsInitParam(desc = "普通手动最大转向角度(deg)，车队联动不使用")]
     public float MaxManualTheta = 45f;
 
     [AsInitParam(desc = "遥控转向输入幂数")]

MultiWheel/MultiWheelM/FleetRemote.cs

@@ -26,6 +26,7 @@ public partial class CartDefinition
         MultiVehicleManualVx = 0;
         MultiVehicleManualVy = 0;
         MultiVehicleManualVth = 0;
+        MultiVehicleHold = false;
     }
 
     [IOObjectUtility]
@@ -43,7 +44,6 @@ public partial class CartDefinition
         var crabOn = false;   // 蟹行（四轮同向平移）
         var rotateOn = false; // 原地旋转（绕车队中心）
         // 速度比例 0~1，作用于摇杆输出的线速度/横移/角速度。
-        var speedRatio = 1f;
         UseGesture? manip = null;
 
         // 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate（同时打开时蟹行生效）。
@@ -87,7 +87,6 @@ public partial class CartDefinition
                     return;
                 }
 
-                var ratio = Math.Clamp(speedRatio, 0, 1);
                 var px = Math.Clamp(pos.X, -1, 1);
                 var py = Math.Clamp(pos.Y, -1, 1);
                 var mode = CurrentMode();
@@ -95,31 +94,27 @@ public partial class CartDefinition
 
                 if (mode == 2)
                 {
-                    // 原地旋转：pos.X(左右) → 角速度(deg/s)，绕车队中心。
                     MultiVehicleManualVx = 0;
                     MultiVehicleManualVy = 0;
-                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                    MultiVehicleManualVth = px;
                 }
                 else if (mode == 1)
                 {
-                    // 蟹行：pos.Y → 前后向线速度，pos.X → 横向线速度（m/s）。
+                    MultiVehicleManualVx = py;
-                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                    MultiVehicleManualVy = px;
-                    MultiVehicleManualVy = px * MaxManualSpeed * ratio;
                     MultiVehicleManualVth = 0;
                 }
                 else
                 {
-                    // 常规：pos.Y → 线速度(m/s)，pos.X → 转向角(deg)。
+                    MultiVehicleManualVx = py;
-                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
                     MultiVehicleManualVy = 0;
-                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                    MultiVehicleManualVth = px;
                 }
-
                 if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
                 {
                     _fleetDiagLastStick = DateTime.Now;
-                    FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
+                    FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} raw " +
-                              $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
+                              $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.000} en={MultiVehicleManualEnabled}");
                 }
             }
         });
@@ -169,15 +164,6 @@ public partial class CartDefinition
             }
         });
 
-        manip.AddWidget(new UseGesture.ThrottleWidget
-        {
-            name = "fleet_speed_ratio",
-            text = "速度比例",
-            position = "50%+10px, 64%+10px",
-            size = "37.5%-10px, 10%-10px",
-            bounceBack = false,
-            OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
-        });
 
         manip.AddWidget(new UseGesture.ButtonWidget
         {
@@ -218,8 +204,7 @@ public partial class CartDefinition
             pb.SeparatorText("状态");
             var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
             pb.Label($"车队联动: {MultiVehicleManualEnabled}  模式: {modeName}");
-            pb.Label($"速度比例: {speedRatio:0.00}");
+            pb.Label($"VxRatio={MultiVehicleManualVx:0.000} VyRatio={MultiVehicleManualVy:0.000}, VthRatio={MultiVehicleManualVth:0.000}");
-            pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
             pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
             pb.Label("请勿同时打开「手动控制」面板");
             pb.Panel.Repaint();

MultiWheel/MultiWheelS/MultiVehicleCar.cs

@@ -1,22 +0,0 @@
-using System.Threading.Tasks;
-using SimpleComposer.RCS;
-using SimpleCore.PropType;
-
-namespace MultiWheelS
-{
-    [CarType(Name = "多车联动AGV")]
-    public class MultiVehicleCar : GhostCar
-    {
-        public bool MultiVehicleSync = true;
-
-        public static new async Task<MultiVehicleCar> Create()
-        {
-            return new MultiVehicleCar
-            {
-                lstatus = "连接中",
-                address = "127.0.0.1",
-                name = "联动AGV"
-            };
-        }
-    }
-}

MultiWheel/MultiWheelS/MultiWheelS.csproj

@@ -1,62 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
-  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
-  <PropertyGroup>
-    <LangVersion>latest</LangVersion>
-  </PropertyGroup>
-  <PropertyGroup>
-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
-    <ProjectGuid>{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}</ProjectGuid>
-    <OutputType>Library</OutputType>
-    <RootNamespace>MultiWheelS</RootNamespace>
-    <AssemblyName>MultiWheelS</AssemblyName>
-    <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
-    <FileAlignment>512</FileAlignment>
-    <Deterministic>true</Deterministic>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
-    <OutputPath>bin\Debug\</OutputPath>
-    <DefineConstants>DEBUG;TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
-    <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
-    <OutputPath>bin\Release\</OutputPath>
-    <DefineConstants>TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-  </PropertyGroup>
-  <ItemGroup>
-    <Reference Include="LessokajiWeaverUtilities">
-      <HintPath>D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll</HintPath>
-    </Reference>
-    <Reference Include="RefSimpleCore">
-      <HintPath>D:\MDCS\Dependencies\Simple\RefSimpleCore.dll</HintPath>
-    </Reference>
-    <Reference Include="SimpleComposer">
-      <HintPath>D:\MDCS\Executables\Simple\SimpleComposer.exe</HintPath>
-    </Reference>
-    <Reference Include="System" />
-    <Reference Include="System.Core" />
-  </ItemGroup>
-  <ItemGroup>
-    <Compile Include="MultiVehicleCar.cs" />
-  </ItemGroup>
-  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
-  <PropertyGroup>
-    <PostBuildEvent>if not exist "$(SolutionDir)build\Simple\plugins" mkdir "$(SolutionDir)build\Simple\plugins"
-if not exist "$(SolutionDir)build\Simple" mkdir "$(SolutionDir)build\Simple"
-xcopy "$(TargetDir)$(TargetFileName)" "$(SolutionDir)build\Simple\plugins" /y
-xcopy "$(TargetDir)$(TargetName).pdb" "$(SolutionDir)build\Simple\plugins" /y
-copy /Y "D:\MDCS\Executables\Simple\SimpleComposer.exe" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Release\MDCSToolBox.dll" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Release\CommonUsage.dll" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Dependencies\Simple\RefSimpleCore.dll" "$(SolutionDir)build\Simple\"</PostBuildEvent>
-  </PropertyGroup>
-</Project>

Tutorial.sln

@@ -1,4 +1,3 @@
-
 Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio Version 17
 VisualStudioVersion = 17.5.33424.131
@@ -15,8 +14,6 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelM", "MultiWheel\M
 EndProject
 Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelC", "MultiWheel\MultiWheelC\MultiWheelC.csproj", "{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}"
 EndProject
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MultiWheelS", "MultiWheel\MultiWheelS\MultiWheelS.csproj", "{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}"
-EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -39,10 +36,6 @@ Global
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.Build.0 = Release|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.Build.0 = Debug|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.ActiveCfg = Release|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE
@@ -52,7 +45,6 @@ Global
 		{A1B2C3D4-E5F6-7890-ABCD-EF1234567890} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E1}
 		{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
 	EndGlobalSection
 	GlobalSection(ExtensibilityGlobals) = postSolution
 		SolutionGuid = {EF6B92D5-5691-42DE-8B38-C17AE7DD7418}

docs/MultiVehicleAutoSyncReview.md

@@ -127,7 +127,7 @@ lock (MultiVehicleFleet)
 编队间距配置变更时，实际控制点半径仍固定 510mm，补偿/转向几何与 `TestCarSyncDistance` 不一致，调参困难。
 
 **建议修复**  
-统一使用 `syncDistance / 2f` 或配置项 `MultiVehicleControlRadius`，删除 magic number 510。
+统一使用 `syncDistance / 2f`，删除 magic number 510。
 
 ---
 
@@ -203,7 +203,7 @@ lock (MultiVehicleFleet)
 - **B**：`MultiVehicleSendMotion` 回调写入 `MultiVehicleAutoCmdTime`；主车自动分支按 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 判定命令新鲜度，超时清零速度/idealPos 并关闭 `AutoEnabled`，避免末速度滑行。
 - **C**：新增本地 `_multiVehicleFleetSeen` 存活时刻表，register/notify 收到即刷新；主车 Tick `PruneStaleFleetMembers()` 按 `MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员，`fleetReady`(数量==总数) 因此蕴含全员新鲜。
 - **D**：回调不再丢弃 `idealPos/idealAngle`，写入 `MultiVehicleAutoIdeal*` 并经 notify(`HasIdeal/IdealX/Y/Th`) 广播；自动模式下以理想车队中心作为各车 layout 前馈目标（`MultiVehicleAutoUseIdealCenter`，默认开）。
-- **E**：新增 `MultiVehicleControlRadius`(0=syncDistance/2)，`ControlPointRadius` 与 `SendMotion(localControlRadius)` 统一取该值，删除硬编码 510。
+- **E**：`ControlPointRadius` 与 `SendMotion(localControlRadius)` 统一取 `syncDistance / 2f`，删除硬编码 510。
 - **F**：notify 改为 POST + JSON body（取代 GET query 串）；新增单调递增 `Seq`，从车丢弃乱序旧包（含主车重启回退识别）。
 - **G**：新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用，`PublishFleetCenter` 整体赋值，控制器线程 `GetFleetCenterSnapshot()` 只读完整快照，消除 torn read。
 - **H**：自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心（定位丢失）时强制停车，补上纯 SLAM 模式安全网；手动模式不受限。

docs/MultiVehicleConfig.md

@@ -94,17 +94,18 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 
 ## 4. 手动遥控（Medulla 车队遥控）
 
-遥控在 Medulla 侧产生指令（`MultiVehicleManual*`，`[AsLowerIO]` 上报 Clumsy）。摇杆输出已是物理单位，Clumsy 端系数应保持 **1（直通）**。
+遥控在 Medulla 侧产生指令（`MultiVehicleManual*`，`[AsLowerIO]` 上报 Clumsy）。车队联动遥控只上报归一化摇杆比例 `[-1,1]`；实际速度/舵角/角速度统一在 Clumsy 的 `FleetManual*` 参数中换算，避免 Medulla 和 Clumsy 两层缩放叠加。
 
 | 端 | 字段 | 含义 | 推荐值 |
 |----|------|------|--------|
-| Medulla `[AsInitParam]` | `MaxManualSpeed` | 车队手动**最大线速度(m/s)** | `0.3` |
+| Medulla `[AsLowerIO]` | `MultiVehicleManualVx/Vy/Vth` | 车队遥控摇杆比例（仅 `[-1,1]`，不带物理单位） | 摇杆值 |
-| Medulla `[AsInitParam]` | `MaxManualAngularSpeed` | 车队手动**最大转向角(deg)** | `45` |
+| Clumsy `clumsy.json` | `FleetManualMaxSpeed` | 满杆线速度(m/s) | `0.3` |
-| Clumsy `clumsy.json` | `ManualCarSyncVxFac` | 手动 Vx 系数（**保持 1，勿再缩放**） | `1.0` |
+| Clumsy `clumsy.json` | `FleetManualMaxSteerAngleDeg` | 常规模式满杆转向舵角(deg) | `45` |
-| Clumsy `clumsy.json` | `ManualCarSyncVthFac` | 手动 Vth 系数（**保持 1**） | `1.0` |
+| Clumsy `clumsy.json` | `FleetManualMaxCrabAngleDeg` | 蟹行模式满杆蟹行舵角(deg) | `60` |
+| Clumsy `clumsy.json` | `FleetManualMaxRotateOmegaDegPerSec` | 原地旋转模式满杆角速度(deg/s) | `45` |
 | Clumsy `clumsy.json` | `SyncThAccPerSec` | 转向角爬升速率(deg/s) | `30` |
 
-> 历史坑：`ManualCarSyncVxFac=0.2 × MaxManualSpeed=0.3 → 0.06 m/s`，肉眼几乎不动；`VthFac=5 → 225°` 超舵轮范围。已统一为系数=1。
+> 历史坑：旧链路会出现 `ManualCarSyncVxFac × MaxManualSpeed` 叠乘，导致满杆只有 `0.06 m/s` 这类异常低速；现在车队联动不再使用 Medulla 的 `MaxManualSpeed/MaxManualAngularSpeed`，这些字段只属于普通手动遥控。
 
 操作：在 Medulla 打开车体工具 **「FleetRemote / 车队联动遥控」**（workspace 摇杆，松手自动回零），开「车队联动」开关后拖摇杆即可。主车摇杆驱动全队；从车由主车广播自动跟随，**无需**各自开开关。**不要**同时打开普通「手动控制」面板（会抢占优先级）。
 
@@ -212,8 +213,10 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 "TwoLegGuessX": -1600,
 "MultiVehicleFleetNum": 2,
 "MultiVehicleUseDetect": true,
-"ManualCarSyncVxFac": 1.0,
+"FleetManualMaxSpeed": 0.3,
-"ManualCarSyncVthFac": 1.0,
+"FleetManualMaxSteerAngleDeg": 45,
+"FleetManualMaxCrabAngleDeg": 60,
+"FleetManualMaxRotateOmegaDegPerSec": 45,
 "SyncThAccPerSec": 30,
 "MultiVehicleMasterEndpoint": "/",            // 从车: "127.0.0.1:8008"
 "PlaygroundRobotName": "agv_multi_1",          // 从车: "agv_multi_2"

docs/停车机器人产品标准化规划书_V1.0.md

@@ -0,0 +1,332 @@
+# 停车机器人产品标准化规划书
+
+## 文档信息
+
+| 项目 | 内容 |
+|------|------|
+| 产品名称 | P2800 停车机器人 |
+| 文档版本 | V1.0 |
+| 编写日期 | 2026-07-14 |
+| 文档类型 | 产品规划书 |
+
+---
+
+# 一、项目目标
+
+## 1.1 总体目标
+
+将停车机器人建设成为具备标准化交付能力的产品，实现车辆自主识别、自主搬运及双车协同控制，逐步形成可复制、可推广、可持续迭代的产品体系。
+
+## 1.2 阶段目标
+
+### **短期目标（7.15~9.15）**
+
+- 保证固定场景下稳定完成车辆搬运演示
+- 提升现有算法稳定性
+- 补齐路径规划能力
+- 完善双车联动基础功能
+
+### **中期目标（9.15~11.30）**
+
+- 引入3D相机
+- 提升复杂场景适应能力
+- 建立稳定性测试体系
+
+### **长期目标（11.30~12.31）**
+
+- 车辆自主识别、自主搬运及双车稳定协同控制
+
+---
+
+# 二、系统现状
+
+目前停车机器人整体流程如下：
+
+```text
+轮胎识别
+    │
+    ▼
+轮胎定位
+    │
+    ▼
+钻车控制
+    │
+    ▼
+双车协同搬运
+```
+
+| 模块 | 当前状态 | 存在问题 |
+|------|----------|----------|
+| 雷达轮胎识别 | 已完成 | 识别误差最大±20mm，稳定性不足 |
+| 路径规划 | 未完成 | 当前仅目标跟踪，无规划能力 |
+| 钻车控制 | 已完成 | 对车辆停放姿态适应能力不足 |
+| 双车联动 | 已完成部分功能 | 横移、原地旋转能力缺失 |
+
+---
+
+# 三、产品演进规划
+
+## 第一阶段：展会保障（7.15~9.15）
+
+### 3.1 雷达轮胎识别优化
+
+保持当前3D雷达方案，不进行硬件更换。
+
+优化方向：
+
+- 提升识别稳定性
+
+目标：
+
+- 连续识别稳定
+
+---
+
+### 3.2 路径规划建设
+
+新增停车机器人路径规划模块。
+
+整体流程：
+
+```text
+轮胎识别
+    ↓
+目标位姿生成
+    ↓
+路径规划
+    ↓
+轨迹跟踪
+    ↓
+钻车控制
+```
+
+建设内容：
+
+- 固定场景路径规划
+- 钻车轨迹生成
+- 轨迹跟踪控制
+
+预计开发周期：约1个月。
+
+---
+
+### 3.3 双车联动完善
+
+完成手动模式：
+
+- 自由运动
+- 横移
+- 原地旋转
+- 任意角度斜行
+
+优化自动斜行稳定性。
+
+阶段交付目标：
+
+完成上海展会1：1场景下的稳定搬运。
+
+---
+
+## 第二阶段：产品完善（9月~年底）
+
+### 轮胎识别升级
+
+技术路线：
+
+```text
+3D雷达
+    ↓
+3D相机（规则识别）
+    ↓
+数据采集
+    ↓
+模型训练
+    ↓
+AI轮胎识别
+```
+
+说明：
+
+优先验证3D相机点云质量；若点云质量满足要求，可先替代雷达方案，再逐步推进深度识别算法。
+
+### 路径规划升级
+
+完善：
+
+- 自动规划
+- 轨迹优化
+- 自动纠偏
+- 障碍物绕行（预研）
+
+### 双车协同升级
+
+实现自动模式：
+
+- 自动横移
+- 自动原地旋转
+- 自动姿态调整
+- 双车同步控制
+
+---
+
+## 第三阶段：产品智能化（长期）
+
+建设统一算法平台。
+
+包括：
+
+- 3D相机AI识别
+- 模型持续优化
+- 多车型适配
+
+形成持续迭代能力。
+
+---
+
+# 四、稳定性建设
+
+稳定性测试贯穿整个研发周期。
+
+## 感知稳定性
+
+验证：
+
+- 不同车型
+- 不同轮胎尺寸
+- 杂物遮挡
+- 点云噪声
+- 光照变化（相机阶段）
+
+统计：
+
+- 识别成功率
+- 定位误差
+- 重复性
+
+## 钻车稳定性
+
+验证：
+
+- 左右偏移
+- 前后偏移
+- 初始角度偏差
+- 不同停车姿态
+
+验证规划算法鲁棒性。
+
+## 双车协同稳定性
+
+验证：
+
+- 横向偏差
+- 纵向偏差
+- 姿态误差
+
+验证自动纠偏能力和稳定双车联动能力。
+
+## 长时间运行测试
+
+开展：
+
+- 连续搬运测试
+- 连续运行测试
+- 异常恢复测试
+
+确保满足工程交付要求。
+
+---
+
+# 五、阶段里程碑
+
+| 时间节点 | 目标 |
+|-----------|------|
+| 9月 | 完成上海展会1：1场景下的稳定演示 |
+| 年底 | 完成路径规划、双车联动及稳定性建设 |
+| 长期 | 完成3D相机替代、AI识别及产品智能化 |
+
+
+
+
+
+然后你再帮我写一个停车机器人目标达成指标的一个MD文档，里面需要包含达成了短/中/长期目标的一些能力：
+
+**短期目标需要达到：**
+
+车端能力：
+
+1.实现钻车/出车过程中的路径规划能力；
+
+2.实现在AGV相对车体有略微偏移(角度≤3°、横向偏移≤50mm)的情况下能够成功规划路径并保证钻入过程无碰撞；
+
+3.实现钻入过程平滑无卡顿；
+
+4.实现在2D雷达/3D雷达/3D相机(非Learning)识别的前提下：识别—规划—控制钻车到位停止精度要达到±15mm/1°以内，重复性测试须达到至少N次；
+
+5.实现钻车过程从开始识别至AGV到位，钻两对轮需满足30s内完成，钻一对轮需满足18s内完成；
+
+6.实现双车联动(带载/空载)手动遥控器控制模式下(0.1m/s~0.3m/s)的自由运动、任意角度斜行、横移以及原地旋转；
+
+7.实现双车联动(带载/空载)自动控制模式下(0.1m/s~0.3m/s)的任意角度稳定斜行；
+
+8.实现双车联动(带载/空载)自动控制模式下斜行到位精度达到±10mm/1°以内；
+
+9.实现双车联动(带载/空载)手/自动控制模式下在两车之间有略微偏移(角度≤2°，横向偏移≤30mm)的情况下依旧能够稳定完成移动操作；
+
+10.稳定完成上海展会同场景下的循环搬运任务，保证任务成功率为100%；
+
+注意：双车联动功能无论是基于网络信号较好的Simple服务器转发或双车直连通信模块，都需要实现前面提到的能力。
+
+调度能力：
+
+1.实现在收到搬车任务后，在没有开启车端避障的前提下，两台AGV的运动过程不发生碰撞；
+
+2.实现在收到搬车任务后，两台AGV的运动需要相对同步，不允许出现锁点问题导致两台AGV距离过远；
+
+3.实现在收到搬车任务后，两台AGV能够分别从车头和车尾同时钻入；
+
+
+
+**中期目标需达到：**
+
+在短期目标能力的基础上额外实现：
+
+车端能力：
+
+1.实现钻车过程从开始识别至AGV到位，钻两对轮需满足20s内完成，钻一对轮需满足12s内完成；
+
+2.实现在AGV相对车体有较大偏移(角度≤6°、横向偏移≤100mm)的情况下能够成功规划并保证钻入过程无碰撞；
+
+3.实现在3D相机(Learning)识别的前提下：识别—规划—控制钻车到位停止精度要达到±5mm/0.5°以内，重复性测试须达到至少N次；
+
+4.实现从车体侧面的钻车能力；
+
+5.实现双车联动(带载/空载)自动控制模式下(0.3m/s~1.0m/s)的自由运动、任意角度斜行、横移以及原地旋转；
+
+6.实现双车联动(带载/空载)自动控制模式下双车通讯、定位异常及其他异常时两车同时立即停车的安全机制；
+
+7.实现双车联动(带载/空载)手/自动控制模式下在两车之间有略微偏移(角度≤5°，横向偏移≤50mm)的情况下依旧能够稳定完成移动操作；
+
+调度能力：
+
+1.实现在收到搬车任务后，若场景内有超过两台以上的AGV，根据AGV状态自动选择最合适执行当前搬运任务的两台AGV；
+
+1.实现在收到搬车任务后，根据两台AGV的状态自动判断二者钻入顺序，同时保证第一台AGV以车头传感器识别的形式钻入，第二台AGV以车尾传感器识别的形式钻入；
+
+
+
+**长期目标须达到：**
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

docs/停车机器人产品能力达成指标_V1.0.md

@@ -0,0 +1,127 @@
+# 停车机器人产品能力达成指标（V1.0）
+
+> 本文档定义停车机器人产品在短期、中期及长期三个阶段应达到的核心能力指标，作为产品研发、测试验收及版本演进依据。
+
+---
+
+# 一、短期目标（展会交付版本）
+
+## 1. 车端能力
+
+### 1.1 钻车能力
+
+- 基于2D雷达/3D雷达/3D相机（非Learning）完成轮胎稳定识别；
+- 建立钻车、出车完整路径规划能力；
+- 完成"识别→目标生成→路径规划→轨迹跟踪→停车"闭环；
+- 重复定位测试≥100次，满足到位精度≤±15 mm，姿态误差≤±1°，成功率≥99%；
+- 实现从开始识别至AGV到位，钻两对轮需满足30s内完成，钻一对轮需满足18s内完成；
+- 当AGV相对车辆存在角度≤3°、横向偏移≤50 mm时，可自动规划无碰撞轨迹；
+- 支持轨迹平滑、速度连续，无明显急停、倒车抖动及振荡；
+- 钻车过程中绝不允许发生机械干涉；
+- 稳定完成上海展会同场景下的循环搬运任务，保证任务成功率为100%；
+
+### 1.2 双车联动能力
+
+支持带载/空载：
+
+- 手动(0.1m/s~0.3m/s)：自由运动、横移、任意角度斜行、原地旋转；
+- 自动(0.1m/s~0.3m/s)：稳定任意角度斜行；
+- 自动斜行停止精度≤±10 mm / ±1°；
+- 两车存在角度≤2°、横向误差≤30 mm安装误差时仍可稳定协同；
+- 支持Simple服务器转发及点对点通信两种模式；
+
+### 1.3 工程能力
+
+- 全过程日志记录；
+- 异常时能够人工接管；
+- 故障定位能力。
+
+---
+
+## 2. 调度能力
+
+- 在没有开启车端避障的前提下，车体间运动不干涉；
+- 车体间的运动需要相对同步，不允许出现锁点问题导致的两台AGV距离过远；
+- 两台AGV能够分别从车头和车尾同时钻入；
+
+---
+
+# 二、中期目标（产品化版本）
+
+在短期目标基础上新增：
+
+## 1. 车端能力
+
+### 1.1 钻车能力
+
+- 引入3D相机Learning算法；
+- 到位精度≤±5 mm、±0.5°；
+- 支持不同车型自动识别；
+- 支持车辆侧向钻车。
+
+- 偏移≤6°、≤100 mm条件下完成自主规划；
+- 支持复杂停车姿态自动修正。
+- 实现从开始识别至AGV到位，钻两对轮需满足20s内完成，钻一对轮需满足12s内完成；
+
+### 1.2 双车联动能力
+
+- 自动模式支持自由运动、横移、斜行、原地旋转（0.3~1.0 m/s）；
+- 通信、定位异常或其他异常时同步急停；
+- 两车≤5°、≤50 mm误差下稳定协同。
+
+## 2. 调度能力
+
+- 多AGV自动选车；
+- 自动确定钻车顺序；
+- 多任务调度；
+- AGV健康状态参与调度决策。
+
+---
+
+# 三、长期目标（标准化产品）
+
+## 1. 智能车端
+
+- AI轮胎识别持续学习；
+- 自动识别车型、轮胎规格、车辆姿态；
+- 多传感器融合感知（雷达+3D相机）。
+
+- 任意方向自主钻车；
+- 动态环境自主避障；
+
+- 双车高速协同（≥1.0 m/s）；
+- 全自动搬运无需人工干预；
+- 故障降级、自恢复、重新编队。
+
+## 2. 智能调度
+
+- 停车场级多机器人调度；
+- 全局交通管理；
+- 自动充电、自动换班；
+- 云端监控与远程运维；
+- 跨停车场统一调度。
+
+## 3. 产品平台能力
+
+- 算法平台化；
+- 数据闭环（采集→标注→训练→部署）；
+- 自动化回归测试；
+- 多车型快速适配。
+
+---
+
+# 四、产品成熟度目标
+
+| 阶段 | 产品定位 | 核心目标 |
+|------|----------|----------|
+| 短期 | Demo交付 | 展会稳定演示，形成基础闭环 |
+| 中期 | 工程产品 | 满足工程交付及批量部署 |
+| 长期 | 标准产品 | 智能化、多车型、多机器人停车搬运平台 |
+
+
+
+# 五、说明
+
+本文档为停车机器人能力建设阶段的目标达成指标说明，用于明确阶段性能力建设方向与评估基准，并非最终产品定版标准。
+
+文档中所列精度、耗时、偏移容差、重复性次数等定量指标，以及部分定性能力描述，将依据实际研发验证、现场测试数据、产品定位变更及项目交付要求进行动态调整。如指标发生变更，以后续正式发布的修订版本或相关专项技术方案为准。