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

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