Fix FleetCrabWalk startup synchronization

2026-07-04 16:05:11 +08:00
parent 8e3f276a24
commit da3d59972c
6 changed files with 151 additions and 89 deletions
@@ -2,6 +2,7 @@ using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Sensors;
 using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
 using MDCSToolBox.Clumsy.MotionControllers;
@@ -292,18 +293,13 @@ namespace MultiWheelC
                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
            }.Get()).Wait();
        }
-        // 车队联动-自动蟹行：供调度按绝对起终点触发。
-        // CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
-            float speed, float CarDirectionBias)
+            float speed)
        {
            var dx = dstX - srcX;
            var dy = dstY - srcY;
            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
-            DLog.Log(
-                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
-                $"len={pathLength:0.0}, speed={speed:0.000}, CarDirectionBias={CarDirectionBias:0.0})",
-                "FleetCrabDbg");

            if (pathLength <= 1f)
            {
@@ -311,6 +307,23 @@ namespace MultiWheelC
                return;
            }

+            var self = PilotDefinition.Self;
+            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
+                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+            {
+                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
+                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
+                return;
+            }
+
+            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
+            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0})",
+                "FleetCrabDbg");
+
            if (dstId != -1)
            {
                while (!TryLock(dstId))
@@ -322,14 +335,10 @@ namespace MultiWheelC

            var action = new MultiWheelC.FleetCrabWalk
            {
-                UseAbsolutePath = true,
-                PathStartX = srcX,
-                PathStartY = srcY,
-                PathEndX = dstX,
-                PathEndY = dstY,
+                CrabAngleDeg = crabAngle,
+                BodyToPathAngleDeg = crabAngle,
                CrabLengthMm = pathLength,
                CrabSpeed = speed,
-                BodyToPathAngleDeg = -CarDirectionBias,
                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
@@ -352,7 +361,6 @@ namespace MultiWheelC
            }
        }
        
-
        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
        {
            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
@@ -447,14 +447,6 @@ public class FleetCrabWalk : MovementDefinition
    /// <summary>路径长度(mm)。</summary>
    public float CrabLengthMm = 2000f;

-    /// <summary>使用调度给定的绝对起终点路径，而不是用当前车队中心和 CrabAngleDeg 生成路径。</summary>
-    public bool UseAbsolutePath = false;
-
-    public float PathStartX;
-    public float PathStartY;
-    public float PathEndX;
-    public float PathEndY;
-
    /// <summary>行驶速度(m/s)。</summary>
    public float CrabSpeed = 0.2f;

@@ -663,7 +655,7 @@ public class FleetCrabWalk : MovementDefinition
            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
-            $"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} pathAngle={CrabAngleDeg:0.0} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
            $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
            "FleetCrabDbg");
@@ -687,40 +679,17 @@ public class FleetCrabWalk : MovementDefinition

        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");

-        Vector2 pathStart;
-        Vector2 dst;
-        float pathLengthMm;
-        float phi;
-        if (UseAbsolutePath)
-        {
-            pathStart = new Vector2(PathStartX, PathStartY);
-            dst = new Vector2(PathEndX, PathEndY);
-            var pathVec = dst - pathStart;
-            pathLengthMm = pathVec.Length();
-            if (pathLengthMm <= 1f)
-            {
-                DLog.Log(
-                    $"ABORT: absolute path is too short src=({PathStartX:0},{PathStartY:0}) dst=({PathEndX:0},{PathEndY:0}) len={pathLengthMm:0.0}",
-                    "FleetCrabDbg");
-                Hedingben.ToastText("车队蟹行路径长度过短", "FleetCrab");
-                yield break;
-            }
-            phi = CommonMath.RoundTh((float)(Math.Atan2(pathVec.Y, pathVec.X) / Math.PI * 180.0));
-        }
-        else
-        {
-            pathStart = new Vector2(x0, y0);
-            pathLengthMm = CrabLengthMm;
-            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
-            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
-        }
+        var pathStart = new Vector2(x0, y0);
+        var pathLengthMm = CrabLengthMm;
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
        var phiRad = phi / 180.0 * Math.PI;
        var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
        var pathLeft = new Vector2(-pathDir.Y, pathDir.X);

        DLog.Log(
-            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode={(UseAbsolutePath ? "absolute" : "relative")} " +
+            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
@@ -728,6 +697,16 @@ public class FleetCrabWalk : MovementDefinition
            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
            "FleetCrabDbg");

+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+            ? conf.MultiVehicleControlRadius
+            : conf.TestCarSyncDistance / 2f));
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
+        var warmStart = DateTime.Now;
+        var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptVx = 0;
@@ -735,19 +714,23 @@ public class FleetCrabWalk : MovementDefinition
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleAutoEnabled = true;
        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
        self.MultiVehicleAutoIdealX = pathStart.X;
        self.MultiVehicleAutoIdealY = pathStart.Y;
        self.MultiVehicleAutoIdealTh = targetBodyTh;
        self.MultiVehicleAutoHasIdeal = true;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
        self.PrimeMasterAutoFromSlam();
-        DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
+        DLog.Log(
+            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");

-        var warmEnd = DateTime.Now.AddSeconds(2.0);
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
        var warmIter = 0;
        var warmReady = false;
+        var warmDetail = "";
        while (!_stopping && DateTime.Now < warmEnd)
        {
            warmIter++;
@@ -755,8 +738,8 @@ public class FleetCrabWalk : MovementDefinition
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = 0;
-            self.MultiVehicleAutoRearTh = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
            self.MultiVehicleAutoIdealX = pathStart.X;
            self.MultiVehicleAutoIdealY = pathStart.Y;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
@@ -769,26 +752,32 @@ public class FleetCrabWalk : MovementDefinition
            if (warmIter % 5 == 0)
                DLog.Log(
                    $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
-                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
+                    $"detail={warmDetail}",
                    "FleetCrabDbg");
-            if (cnt >= conf.MultiVehicleFleetNum)
+            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
            {
                warmReady = true;
                DLog.Log(
-                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
                    "FleetCrabDbg");
                break;
            }
            yield return true;
        }
        if (!warmReady)
-            DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
+        {
+            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
                "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+            Cleanup();
+            yield break;
+        }

        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");

-        if (warmReady && !UseAbsolutePath &&
-            self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
        {
            x0 = warmX;
            y0 = warmY;
@@ -819,13 +808,6 @@ public class FleetCrabWalk : MovementDefinition
        var accel = Math.Abs(FleetCrabAccel);
        var cmdSpeed = 0f;
        var lastTick = DateTime.Now;
-        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
-            ? conf.MultiVehicleControlRadius
-            : conf.TestCarSyncDistance / 2f));
-        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
-            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
-            out _, out _);
        var stopReason = "done";

        while (!_stopping)
@@ -185,6 +185,12 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
    public float FleetCrabGcpThetaThreshold = 95f;

+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
    // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
    [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
    public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
@@ -280,10 +280,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        if (!rotateActive)
        {
            _multiVehicleRotateModeActive = false;
+            if (!active)
+            {
                MultiVehicleRotateWheelsReady = true;
+                _multiVehicleRotateAlignDetail = "";
+            }
            MultiVehicleRotateFleetReady = true;
            _multiVehicleRotateDirectionHint = 1f;
-            _multiVehicleRotateAlignDetail = "";
            return;
        }

@@ -1309,10 +1312,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _mvRotCompVx = 0;
                _mvRotCompVy = 0;
                _mvRotCompOmega = 0;
-                MultiVehicleRotateWheelsReady = true;
                MultiVehicleRotateFleetReady = true;
-                _multiVehicleRotateAlignDetail = "";
-                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
+                // Reset rotate-only PI state outside rotate mode.
                _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                _rotPiLastTime = DateTime.MinValue;
                _rotPoseEpisode = false;
@@ -1323,6 +1324,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)
                {
@@ -1335,6 +1345,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}");
            }
        }
@@ -1631,15 +1642,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) 唯一确定：
-        // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
+        MultiVehicleRotateWheelsReady = false;
+        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,
@@ -1664,7 +1724,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            MotionFeasible = _multiVehicleMotionFeasible,
            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
            RotateWheelsAligned = MultiVehicleRotateWheelsReady,
-            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail,
+            AppliedNotificationSeq = _multiVehicleAppliedSeq
        };
    }

@@ -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;
    }

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