贯通车队运行链并支持轨迹自动推导β

MultiWheelC/Fleet/FleetCoordinator.cs

@@ -2,7 +2,7 @@ using System;
 using System.Collections.Generic;
 using MultiWheelC.Trajectory;
 using MyParking.Shared;
-
+// 负责运动中：每周期计算每辆车的速度命令
 namespace MultiWheelC.Fleet
 {
     // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。
@@ -173,6 +173,10 @@ namespace MultiWheelC.Fleet
 
         public string LastFailureReason { get; private set; }
 
+        // 运动前成员β准备必须与车队控制器采用同一个车队运动方向。
+        public double MotionDirectionInFleetRadians =>
+            _fleetController.MotionDirectionInFleetRadians;
+
         public void Start(
             FleetLayout layout,
             Trajectory2D trajectory)

MultiWheelC/Fleet/FleetMemberAgent.cs

@@ -30,6 +30,8 @@ namespace MultiWheelC.Fleet
         private readonly double _alignmentStableSeconds;
 
         private double _alignedDurationSeconds;
+        private double? _lastAcceptedCommandTimeSeconds;
+        private double? _commandDeadlineSeconds;
 
         /// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
         public FleetMemberAgent(
@@ -81,6 +83,13 @@ namespace MultiWheelC.Fleet
 
         public string LastFailureReason { get; private set; }
 
+        // 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。
+        public double? LastAcceptedCommandTimeSeconds =>
+            _lastAcceptedCommandTimeSeconds;
+
+        public double? CommandDeadlineSeconds =>
+            _commandDeadlineSeconds;
+
         /// <summary>停车并开始准备本车固定β滚动运动系。</summary>
         public bool BeginRollingPreparation(
             long planId,
@@ -158,8 +167,18 @@ namespace MultiWheelC.Fleet
         }
 
         /// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
-        public bool Activate(long planId)
+        public bool Activate(
+            long planId,
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
         {
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
+
             if (planId != CurrentPlanId)
             {
                 LastFailureReason =
@@ -169,7 +188,9 @@ namespace MultiWheelC.Fleet
 
             if (State == FleetMemberAgentState.Active)
             {
-                return true;
+                // 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。
+                return UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds);
             }
 
             if (State != FleetMemberAgentState.Ready ||
@@ -227,6 +248,9 @@ namespace MultiWheelC.Fleet
             }
 
             State = FleetMemberAgentState.Active;
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
             LastFailureReason = string.Empty;
             return true;
         }
@@ -235,12 +259,20 @@ namespace MultiWheelC.Fleet
         public bool Execute(
             long planId,
             FleetMemberCommand command,
+            double commandReceivedTimeSeconds,
+            double validForSeconds,
             TimeSpan? interval = null)
         {
             ValidatePlanId(planId);
             NumericGuard.EnsureFinite(
                 command.TwistInVehicleBody,
                 nameof(command));
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
 
             if (planId != CurrentPlanId)
             {
@@ -262,6 +294,13 @@ namespace MultiWheelC.Fleet
                     "成员车尚未激活，不能执行速度命令。");
             }
 
+            // 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。
+            if (!UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds))
+            {
+                return false;
+            }
+
             if (!IsCommandCompatibleWithPreparation(
                     command.TwistInVehicleBody))
             {
@@ -289,10 +328,59 @@ namespace MultiWheelC.Fleet
                 return Fail(exception.Message);
             }
 
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
             LastFailureReason = string.Empty;
             return true;
         }
 
+        // 运行循环即使没有收到新命令也必须调用本方法，超时后会本地停车并锁存Faulted。
+        public bool UpdateCommandWatchdog(
+            double currentTimeSeconds)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (State == FleetMemberAgentState.Faulted)
+            {
+                return false;
+            }
+
+            if (State != FleetMemberAgentState.Active)
+            {
+                return true;
+            }
+
+            if (!_lastAcceptedCommandTimeSeconds.HasValue ||
+                !_commandDeadlineSeconds.HasValue)
+            {
+                return Fail(
+                    "成员车已经激活，但本地命令看门狗尚未初始化。");
+            }
+
+            if (currentTimeSeconds <
+                _lastAcceptedCommandTimeSeconds.Value)
+            {
+                return Fail(
+                    "成员车本地单调时钟发生倒退，无法继续校验命令时效。");
+            }
+
+            if (currentTimeSeconds <=
+                _commandDeadlineSeconds.Value)
+            {
+                return true;
+            }
+
+            var commandAgeSeconds =
+                currentTimeSeconds -
+                _lastAcceptedCommandTimeSeconds.Value;
+            return Fail(
+                "成员车等待主车有效命令超时，" +
+                $"最近一次命令距今{commandAgeSeconds:F3}s。");
+        }
+
         /// <summary>正常取消当前任务并立即停止驱动轮。</summary>
         public void Stop()
         {
@@ -302,6 +390,7 @@ namespace MultiWheelC.Fleet
             CurrentPlanId = 0;
             MotionDirectionInBodyRadians = 0.0;
             _alignedDurationSeconds = 0.0;
+            ClearCommandWatchdog();
             LastFailureReason = string.Empty;
         }
 
@@ -323,6 +412,7 @@ namespace MultiWheelC.Fleet
                 State = FleetMemberAgentState.Preparing;
                 LastFailureReason = string.Empty;
                 _alignedDurationSeconds = 0.0;
+                ClearCommandWatchdog();
 
                 var accepted = mode ==
                     FleetMemberPreparationMode.Rolling
@@ -423,6 +513,29 @@ namespace MultiWheelC.Fleet
             return false;
         }
 
+        private void AcceptCommandDeadline(
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
+        {
+            var commandDeadlineSeconds =
+                commandReceivedTimeSeconds +
+                validForSeconds;
+            NumericGuard.EnsureFinite(
+                commandDeadlineSeconds,
+                nameof(validForSeconds));
+
+            _lastAcceptedCommandTimeSeconds =
+                commandReceivedTimeSeconds;
+            _commandDeadlineSeconds =
+                commandDeadlineSeconds;
+        }
+
+        private void ClearCommandWatchdog()
+        {
+            _lastAcceptedCommandTimeSeconds = null;
+            _commandDeadlineSeconds = null;
+        }
+
         /// <summary>锁存成员车故障并立即清零驱动轮速度。</summary>
         private bool Fail(string reason)
         {

MultiWheelC/Fleet/FleetPreparationCoordinator.cs

@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
 using MyParking.Shared;
+// 负责运动前：所有车辆舵轮是否准备完成
 
 namespace MultiWheelC.Fleet
 {

MultiWheelC/Fleet/FleetSafetySupervisor.cs

@@ -0,0 +1,286 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 主车已经接收并接受的一辆成员车安全状态。
+    public readonly struct FleetMemberSafetyStatus
+    {
+        public FleetMemberSafetyStatus(
+            int vehicleId,
+            long planId,
+            bool isStateAvailable,
+            bool isFaulted,
+            int failureCode,
+            double lastAcceptedReportTimeSeconds)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "成员车编号必须大于零。");
+            }
+
+            if (planId < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "任务编号不能为负数。");
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                lastAcceptedReportTimeSeconds,
+                nameof(lastAcceptedReportTimeSeconds));
+
+            VehicleId = vehicleId;
+            PlanId = planId;
+            IsStateAvailable = isStateAvailable;
+            IsFaulted = isFaulted;
+            FailureCode = failureCode;
+            LastAcceptedReportTimeSeconds =
+                lastAcceptedReportTimeSeconds;
+        }
+
+        public int VehicleId { get; }
+
+        public long PlanId { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool IsFaulted { get; }
+
+        // 零表示成员车没有报告结构化故障。
+        public int FailureCode { get; }
+
+        // 使用主车本地单调时钟，不能直接填写从车上传的时间戳。
+        public double LastAcceptedReportTimeSeconds { get; }
+    }
+
+    // 一次安全检查的结果；ShouldStop可直接作为是否停车的判断标识。
+    public readonly struct FleetSafetyDecision
+    {
+        internal FleetSafetyDecision(
+            bool shouldStop,
+            int sourceVehicleId,
+            string reason)
+        {
+            ShouldStop = shouldStop;
+            SourceVehicleId = sourceVehicleId;
+            Reason = reason ?? string.Empty;
+        }
+
+        public bool ShouldStop { get; }
+
+        // 零表示原因属于整个车队，而不是某一辆成员车。
+        public int SourceVehicleId { get; }
+
+        public string Reason { get; }
+    }
+
+    // 检查成员通信和健康状态，并锁存需要整队停车的首个原因。
+    public sealed class FleetSafetySupervisor
+    {
+        private readonly double _communicationTimeoutSeconds;
+
+        private long _activePlanId;
+        private FleetSafetyDecision _latchedDecision;
+
+        public FleetSafetySupervisor(
+            double communicationTimeoutSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                communicationTimeoutSeconds,
+                nameof(communicationTimeoutSeconds));
+
+            _communicationTimeoutSeconds =
+                communicationTimeoutSeconds;
+            Reset();
+        }
+
+        public double CommunicationTimeoutSeconds =>
+            _communicationTimeoutSeconds;
+
+        public long ActivePlanId => _activePlanId;
+
+        public bool IsActive => _activePlanId > 0;
+
+        public bool IsStopLatched =>
+            _latchedDecision.ShouldStop;
+
+        public FleetSafetyDecision LastDecision =>
+            _latchedDecision;
+
+        // 开始一次新任务，同时清除上一任务留下的停车锁存。
+        public void Start(long planId)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "活动任务编号必须大于零。");
+            }
+
+            _activePlanId = planId;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        // 返回ShouldStop；本类只负责判定，实际停车由后续运行入口执行。
+        public FleetSafetyDecision Evaluate(
+            FleetLayout layout,
+            IReadOnlyList<FleetMemberSafetyStatus> memberStatuses,
+            double currentTimeSeconds)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (memberStatuses == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(memberStatuses));
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (!IsActive)
+            {
+                return new FleetSafetyDecision(
+                    true,
+                    0,
+                    "车队安全监督器尚未启动活动任务。");
+            }
+
+            if (IsStopLatched)
+            {
+                return _latchedDecision;
+            }
+
+            var statusesByVehicleId =
+                new Dictionary<int, FleetMemberSafetyStatus>();
+
+            for (var index = 0;
+                 index < memberStatuses.Count;
+                 index++)
+            {
+                var status = memberStatuses[index];
+
+                // 非当前编队成员的状态不参与本次任务安全判定。
+                if (!layout.TryGetVehicle(
+                        status.VehicleId,
+                        out _))
+                {
+                    continue;
+                }
+
+                if (statusesByVehicleId.ContainsKey(
+                        status.VehicleId))
+                {
+                    return LatchStop(
+                        status.VehicleId,
+                        $"成员车{status.VehicleId}存在重复状态报告。");
+                }
+
+                statusesByVehicleId.Add(
+                    status.VehicleId,
+                    status);
+            }
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleId =
+                    layout.Vehicles[index].VehicleId;
+
+                if (!statusesByVehicleId.TryGetValue(
+                        vehicleId,
+                        out var status))
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"未收到成员车{vehicleId}的状态报告。");
+                }
+
+                if (status.PlanId != _activePlanId)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告的任务编号" +
+                        $"{status.PlanId}与当前任务" +
+                        $"{_activePlanId}不一致。");
+                }
+
+                if (status.LastAcceptedReportTimeSeconds >
+                    currentTimeSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}的主车接收时间晚于当前时间。");
+                }
+
+                var reportAgeSeconds =
+                    currentTimeSeconds -
+                    status.LastAcceptedReportTimeSeconds;
+                if (reportAgeSeconds >
+                    _communicationTimeoutSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}通信超时，" +
+                        $"最近有效报告距今" +
+                        $"{reportAgeSeconds:F3}s。");
+                }
+
+                if (status.IsFaulted ||
+                    status.FailureCode != 0)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告故障，" +
+                        $"故障码为{status.FailureCode}。");
+                }
+
+                if (!status.IsStateAvailable)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}状态不可用。");
+                }
+            }
+
+            _latchedDecision = CreateContinueDecision();
+            return _latchedDecision;
+        }
+
+        // 结束当前任务并清除锁存；未开始新任务前Evaluate仍会要求停车。
+        public void Reset()
+        {
+            _activePlanId = 0;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        private FleetSafetyDecision LatchStop(
+            int sourceVehicleId,
+            string reason)
+        {
+            _latchedDecision = new FleetSafetyDecision(
+                true,
+                sourceVehicleId,
+                reason);
+            return _latchedDecision;
+        }
+
+        private static FleetSafetyDecision
+            CreateContinueDecision()
+        {
+            return new FleetSafetyDecision(
+                false,
+                0,
+                string.Empty);
+        }
+    }
+}

MultiWheelC/Fleet/IFleetTransport.cs

@@ -0,0 +1,16 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 隔离车队运行逻辑与具体无线、串口或内存传输实现。
+    public interface IFleetTransport
+    {
+        void SendCommand(FleetCommand command);
+
+        void SendReport(FleetMemberReport report);
+
+        bool TryReceiveCommand(out FleetCommand command);
+
+        bool TryReceiveReport(out FleetMemberReport report);
+    }
+}