1011 lines
34 KiB
C#
1011 lines
34 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.StateEstimation;
|
|
using MultiWheelC.Trajectory;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Fleet
|
|
{
|
|
/// <summary>表示本机车队任务运行入口的生命周期阶段。</summary>
|
|
public enum FleetRuntimeState
|
|
{
|
|
Idle = 0,
|
|
Preparing = 1,
|
|
Ready = 2,
|
|
Active = 3,
|
|
Completed = 4,
|
|
Faulted = 5
|
|
}
|
|
|
|
/// <summary>在主车或从车上串联固定布局滚动车队任务。</summary>
|
|
public sealed class FleetRuntime
|
|
{
|
|
private const int RuntimeFailureCode = 1;
|
|
private const int FaultStopReasonCode = 1;
|
|
|
|
private readonly IFleetTransport _transport;
|
|
private readonly FleetMemberAgent _localAgent;
|
|
private readonly IVehicleStateProvider _localStateProvider;
|
|
private readonly FleetPreparationCoordinator _preparationCoordinator;
|
|
private readonly FleetCoordinator _fleetCoordinator;
|
|
private readonly FleetSafetySupervisor _safetySupervisor;
|
|
private readonly double _commandValidForSeconds;
|
|
private readonly double _preparationTimeoutSeconds;
|
|
private readonly Dictionary<int, AcceptedReport> _reports =
|
|
new Dictionary<int, AcceptedReport>();
|
|
private readonly Dictionary<int, long> _lastReportSequences =
|
|
new Dictionary<int, long>();
|
|
|
|
private FleetLayout _activeLayout;
|
|
private VehicleState? _lastLocalState;
|
|
private double? _planStartTimeSeconds;
|
|
private long _nextCommandSequence;
|
|
private long _nextReportSequence;
|
|
private long _lastReceivedCommandSequence;
|
|
private long _lastAppliedCommandSequence;
|
|
|
|
/// <summary>创建本车运行入口；主车必须额外提供三个主车侧组件。</summary>
|
|
public FleetRuntime(
|
|
int selfVehicleId,
|
|
int leaderVehicleId,
|
|
IFleetTransport transport,
|
|
FleetMemberAgent localAgent,
|
|
IVehicleStateProvider localStateProvider,
|
|
FleetPreparationCoordinator preparationCoordinator = null,
|
|
FleetCoordinator fleetCoordinator = null,
|
|
FleetSafetySupervisor safetySupervisor = null,
|
|
double commandValidForSeconds = 0.5,
|
|
double preparationTimeoutSeconds = 10.0)
|
|
{
|
|
if (selfVehicleId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(selfVehicleId));
|
|
}
|
|
|
|
if (leaderVehicleId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(leaderVehicleId));
|
|
}
|
|
|
|
_transport = transport ??
|
|
throw new ArgumentNullException(nameof(transport));
|
|
_localAgent = localAgent ??
|
|
throw new ArgumentNullException(nameof(localAgent));
|
|
_localStateProvider = localStateProvider ??
|
|
throw new ArgumentNullException(nameof(localStateProvider));
|
|
NumericGuard.EnsureFinitePositive(
|
|
commandValidForSeconds,
|
|
nameof(commandValidForSeconds));
|
|
NumericGuard.EnsureFinitePositive(
|
|
preparationTimeoutSeconds,
|
|
nameof(preparationTimeoutSeconds));
|
|
|
|
if (_localAgent.VehicleId != selfVehicleId)
|
|
{
|
|
throw new ArgumentException(
|
|
"本车执行器车号与运行入口车号不一致。",
|
|
nameof(localAgent));
|
|
}
|
|
|
|
SelfVehicleId = selfVehicleId;
|
|
LeaderVehicleId = leaderVehicleId;
|
|
_preparationCoordinator = preparationCoordinator;
|
|
_fleetCoordinator = fleetCoordinator;
|
|
_safetySupervisor = safetySupervisor;
|
|
_commandValidForSeconds = commandValidForSeconds;
|
|
_preparationTimeoutSeconds = preparationTimeoutSeconds;
|
|
|
|
if (IsLeader &&
|
|
(_preparationCoordinator == null ||
|
|
_fleetCoordinator == null ||
|
|
_safetySupervisor == null))
|
|
{
|
|
throw new ArgumentException(
|
|
"主车必须提供准备协调器、车队协调器和安全监督器。");
|
|
}
|
|
|
|
State = FleetRuntimeState.Idle;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
public int SelfVehicleId { get; }
|
|
|
|
public int LeaderVehicleId { get; }
|
|
|
|
public bool IsLeader => SelfVehicleId == LeaderVehicleId;
|
|
|
|
public FleetRuntimeState State { get; private set; }
|
|
|
|
public long CurrentPlanId { get; private set; }
|
|
|
|
public long LastAppliedCommandSequence =>
|
|
_lastAppliedCommandSequence;
|
|
|
|
public string LastFailureReason { get; private set; }
|
|
|
|
public FleetCoordinationCycleOutput LastCoordinationOutput
|
|
{
|
|
get;
|
|
private set;
|
|
}
|
|
|
|
/// <summary>由主车启动一次使用当前控制器固定β的滚动轨迹任务。</summary>
|
|
public bool StartRollingPlan(
|
|
long planId,
|
|
FleetLayout layout,
|
|
Trajectory2D trajectory)
|
|
{
|
|
EnsureLeader();
|
|
if (planId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(planId));
|
|
}
|
|
|
|
if (layout == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(layout));
|
|
}
|
|
|
|
if (trajectory == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(trajectory));
|
|
}
|
|
|
|
if (!layout.TryGetVehicle(SelfVehicleId, out _))
|
|
{
|
|
throw new ArgumentException(
|
|
"主车不在当前车队布局中。",
|
|
nameof(layout));
|
|
}
|
|
|
|
if (State == FleetRuntimeState.Preparing ||
|
|
State == FleetRuntimeState.Active)
|
|
{
|
|
LastFailureReason = "当前车队任务尚未结束。";
|
|
return false;
|
|
}
|
|
|
|
ResetLeaderForNewPlan();
|
|
CurrentPlanId = planId;
|
|
_activeLayout = layout;
|
|
State = FleetRuntimeState.Preparing;
|
|
|
|
try
|
|
{
|
|
_fleetCoordinator.Start(layout, trajectory);
|
|
_preparationCoordinator.StartRollingPreparation(
|
|
planId,
|
|
layout,
|
|
_fleetCoordinator.MotionDirectionInFleetRadians);
|
|
_safetySupervisor.Start(planId);
|
|
|
|
foreach (var target in _preparationCoordinator.Targets)
|
|
{
|
|
var command = NewCommand(
|
|
target.VehicleId,
|
|
FleetCommandKind.PrepareRolling,
|
|
target.MotionDirectionInBodyRadians,
|
|
Twist2D.Zero);
|
|
|
|
if (target.VehicleId == SelfVehicleId)
|
|
{
|
|
if (!_localAgent.BeginRollingPreparation(
|
|
planId,
|
|
target.MotionDirectionInBodyRadians))
|
|
{
|
|
return FaultLeader(
|
|
LocalFailure("主车舵轮准备失败。"));
|
|
}
|
|
|
|
AcceptLocalCommand(command.SequenceNumber);
|
|
}
|
|
else
|
|
{
|
|
_transport.SendCommand(command);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
return FaultLeader(
|
|
"启动车队任务失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
/// <summary>执行一个本机调度周期，并返回更新后的运行阶段。</summary>
|
|
public FleetRuntimeState Update(
|
|
double currentTimeSeconds,
|
|
double deltaTimeSeconds)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
currentTimeSeconds,
|
|
nameof(currentTimeSeconds));
|
|
NumericGuard.EnsureFinitePositive(
|
|
deltaTimeSeconds,
|
|
nameof(deltaTimeSeconds));
|
|
|
|
return IsLeader
|
|
? UpdateLeader(currentTimeSeconds, deltaTimeSeconds)
|
|
: UpdateMember(currentTimeSeconds, deltaTimeSeconds);
|
|
}
|
|
|
|
/// <summary>正常取消当前任务；主车同时向其余成员广播停止。</summary>
|
|
public void Stop(string reason = "")
|
|
{
|
|
if (IsLeader && CurrentPlanId > 0)
|
|
{
|
|
BroadcastStop(reasonCode: 0);
|
|
_preparationCoordinator.Cancel();
|
|
_fleetCoordinator.Cancel();
|
|
_safetySupervisor.Reset();
|
|
}
|
|
|
|
_localAgent.Stop();
|
|
_activeLayout = null;
|
|
CurrentPlanId = FleetProtocol.NoActivePlanId;
|
|
State = FleetRuntimeState.Idle;
|
|
_planStartTimeSeconds = null;
|
|
LastCoordinationOutput = null;
|
|
LastFailureReason = reason ?? string.Empty;
|
|
}
|
|
|
|
private FleetRuntimeState UpdateLeader(
|
|
double currentTimeSeconds,
|
|
double deltaTimeSeconds)
|
|
{
|
|
if (State != FleetRuntimeState.Preparing &&
|
|
State != FleetRuntimeState.Active)
|
|
{
|
|
return State;
|
|
}
|
|
|
|
_planStartTimeSeconds ??= currentTimeSeconds;
|
|
if (!ReceiveReports(currentTimeSeconds))
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds))
|
|
{
|
|
FaultLeader(LocalFailure("主车本地执行失败。"));
|
|
return State;
|
|
}
|
|
|
|
if (!TryReadLocalState(out var localState))
|
|
{
|
|
FaultLeader("主车本地状态不可用。" );
|
|
return State;
|
|
}
|
|
|
|
StoreLocalReport(localState.Value, currentTimeSeconds);
|
|
|
|
if (State == FleetRuntimeState.Preparing)
|
|
{
|
|
UpdatePreparationBarrier();
|
|
if (_preparationCoordinator.State ==
|
|
FleetPreparationCoordinatorState.Faulted)
|
|
{
|
|
FaultLeader(_preparationCoordinator.LastFailureReason);
|
|
return State;
|
|
}
|
|
|
|
if (currentTimeSeconds - _planStartTimeSeconds.Value >
|
|
_preparationTimeoutSeconds)
|
|
{
|
|
FaultLeader("车队舵轮准备超时，未能全员Ready。" );
|
|
return State;
|
|
}
|
|
}
|
|
|
|
if (!CheckLeaderSafety(currentTimeSeconds))
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (State == FleetRuntimeState.Preparing &&
|
|
!ActivateFleet(currentTimeSeconds))
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (State == FleetRuntimeState.Active)
|
|
{
|
|
RunCoordination(currentTimeSeconds, deltaTimeSeconds);
|
|
}
|
|
|
|
return State;
|
|
}
|
|
|
|
private FleetRuntimeState UpdateMember(
|
|
double currentTimeSeconds,
|
|
double deltaTimeSeconds)
|
|
{
|
|
ReceiveCommands(currentTimeSeconds);
|
|
if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds))
|
|
{
|
|
if (State != FleetRuntimeState.Faulted)
|
|
{
|
|
FaultMember(LocalFailure("成员车本地执行失败。"));
|
|
}
|
|
}
|
|
|
|
var hasState = TryReadLocalState(out var localState);
|
|
if (!hasState && CurrentPlanId > 0 &&
|
|
State != FleetRuntimeState.Faulted)
|
|
{
|
|
FaultMember("成员车本地状态不可用。" );
|
|
}
|
|
|
|
SendLocalReport(localState, hasState, currentTimeSeconds);
|
|
return State;
|
|
}
|
|
|
|
private bool TickLocalAgent(
|
|
double currentTimeSeconds,
|
|
double deltaTimeSeconds)
|
|
{
|
|
if (State == FleetRuntimeState.Faulted)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (_localAgent.State == FleetMemberAgentState.Preparing)
|
|
{
|
|
_localAgent.UpdatePreparation(deltaTimeSeconds);
|
|
}
|
|
|
|
if (_localAgent.State == FleetMemberAgentState.Active &&
|
|
!_localAgent.UpdateCommandWatchdog(currentTimeSeconds))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (_localAgent.State == FleetMemberAgentState.Faulted)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!IsLeader)
|
|
{
|
|
State = ToRuntimeState(_localAgent.State);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private bool TryReadLocalState(out VehicleState? state)
|
|
{
|
|
try
|
|
{
|
|
if (_localStateProvider.TryGetState(out var value))
|
|
{
|
|
_lastLocalState = value;
|
|
state = value;
|
|
return true;
|
|
}
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
LastFailureReason =
|
|
"读取本车状态失败：" + exception.Message;
|
|
}
|
|
|
|
state = null;
|
|
return false;
|
|
}
|
|
|
|
private bool ReceiveReports(double currentTimeSeconds)
|
|
{
|
|
try
|
|
{
|
|
while (_transport.TryReceiveReport(out var report))
|
|
{
|
|
if (report.VehicleId == SelfVehicleId ||
|
|
_activeLayout == null ||
|
|
!_activeLayout.TryGetVehicle(report.VehicleId, out _) ||
|
|
report.PlanId != CurrentPlanId)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
ValidateReport(report);
|
|
if (_lastReportSequences.TryGetValue(
|
|
report.VehicleId,
|
|
out var sequence) &&
|
|
report.SequenceNumber <= sequence)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
_lastReportSequences[report.VehicleId] =
|
|
report.SequenceNumber;
|
|
_reports[report.VehicleId] =
|
|
new AcceptedReport(report, currentTimeSeconds);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
return FaultLeader(
|
|
"接收成员报告失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
private void StoreLocalReport(
|
|
VehicleState state,
|
|
double currentTimeSeconds)
|
|
{
|
|
_reports[SelfVehicleId] = new AcceptedReport(
|
|
CreateReport(state, isStateAvailable: true),
|
|
currentTimeSeconds);
|
|
}
|
|
|
|
private void UpdatePreparationBarrier()
|
|
{
|
|
foreach (var accepted in _reports.Values)
|
|
{
|
|
var report = accepted.Report;
|
|
_preparationCoordinator.ReportMemberStatus(
|
|
new FleetMemberPreparationStatus(
|
|
CurrentPlanId,
|
|
report.VehicleId,
|
|
ToAgentState(report.State),
|
|
report.State == FleetMemberState.Faulted
|
|
? $"成员车故障码{report.FailureCode}。"
|
|
: string.Empty));
|
|
}
|
|
}
|
|
|
|
private bool CheckLeaderSafety(double currentTimeSeconds)
|
|
{
|
|
var statuses = new List<FleetMemberSafetyStatus>();
|
|
foreach (var accepted in _reports.Values)
|
|
{
|
|
var report = accepted.Report;
|
|
statuses.Add(new FleetMemberSafetyStatus(
|
|
report.VehicleId,
|
|
report.PlanId,
|
|
report.IsStateAvailable,
|
|
report.State == FleetMemberState.Faulted,
|
|
report.FailureCode,
|
|
accepted.ReceivedTimeSeconds));
|
|
}
|
|
|
|
var graceExpired =
|
|
currentTimeSeconds - _planStartTimeSeconds.Value >
|
|
_safetySupervisor.CommunicationTimeoutSeconds;
|
|
if (statuses.Count != _activeLayout.VehicleCount &&
|
|
!graceExpired)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
var decision = _safetySupervisor.Evaluate(
|
|
_activeLayout,
|
|
statuses,
|
|
currentTimeSeconds);
|
|
return !decision.ShouldStop ||
|
|
FaultLeader(decision.Reason);
|
|
}
|
|
|
|
private bool ActivateFleet(double currentTimeSeconds)
|
|
{
|
|
if (_preparationCoordinator.State !=
|
|
FleetPreparationCoordinatorState.ReadyToActivate)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!_preparationCoordinator.TryAuthorizeActivation(
|
|
CurrentPlanId))
|
|
{
|
|
return FaultLeader("无法授权全队激活。" );
|
|
}
|
|
|
|
var command = NewCommand(
|
|
FleetProtocol.BroadcastVehicleId,
|
|
FleetCommandKind.Activate,
|
|
0.0,
|
|
Twist2D.Zero);
|
|
if (!_localAgent.Activate(
|
|
CurrentPlanId,
|
|
currentTimeSeconds,
|
|
command.ValidForSeconds))
|
|
{
|
|
return FaultLeader(LocalFailure("主车激活失败。"));
|
|
}
|
|
|
|
AcceptLocalCommand(command.SequenceNumber);
|
|
try
|
|
{
|
|
_transport.SendCommand(command);
|
|
State = FleetRuntimeState.Active;
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
return FaultLeader(
|
|
"广播激活命令失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
private void RunCoordination(
|
|
double currentTimeSeconds,
|
|
double deltaTimeSeconds)
|
|
{
|
|
var result = _fleetCoordinator.ExecuteCycle(
|
|
BuildMemberStates(),
|
|
currentTimeSeconds,
|
|
deltaTimeSeconds,
|
|
out var output);
|
|
LastCoordinationOutput = output;
|
|
|
|
if (result == FleetCoordinationCycleResult.CommandGenerated ||
|
|
result == FleetCoordinationCycleResult.WaitingForState)
|
|
{
|
|
Dispatch(output.MemberCommands, currentTimeSeconds);
|
|
return;
|
|
}
|
|
|
|
if (result == FleetCoordinationCycleResult.Completed)
|
|
{
|
|
BroadcastStop(reasonCode: 0);
|
|
_localAgent.Stop();
|
|
_preparationCoordinator.Cancel();
|
|
_safetySupervisor.Reset();
|
|
State = FleetRuntimeState.Completed;
|
|
return;
|
|
}
|
|
|
|
var reason = string.IsNullOrWhiteSpace(output.Reason)
|
|
? _fleetCoordinator.LastFailureReason
|
|
: output.Reason;
|
|
FaultLeader(string.IsNullOrWhiteSpace(reason)
|
|
? "车队协调器未生成可执行命令。"
|
|
: reason);
|
|
}
|
|
|
|
private bool Dispatch(
|
|
IReadOnlyList<FleetMemberCommand> commands,
|
|
double currentTimeSeconds)
|
|
{
|
|
if (commands == null ||
|
|
commands.Count != _activeLayout.VehicleCount)
|
|
{
|
|
return FaultLeader("成员命令数量与布局不一致。" );
|
|
}
|
|
|
|
try
|
|
{
|
|
foreach (var memberCommand in commands)
|
|
{
|
|
var command = NewCommand(
|
|
memberCommand.VehicleId,
|
|
FleetCommandKind.Motion,
|
|
0.0,
|
|
memberCommand.TwistInVehicleBody);
|
|
if (memberCommand.VehicleId == SelfVehicleId)
|
|
{
|
|
if (!_localAgent.Execute(
|
|
CurrentPlanId,
|
|
memberCommand,
|
|
currentTimeSeconds,
|
|
command.ValidForSeconds))
|
|
{
|
|
return FaultLeader(
|
|
LocalFailure("主车执行速度命令失败。"));
|
|
}
|
|
|
|
AcceptLocalCommand(command.SequenceNumber);
|
|
}
|
|
else
|
|
{
|
|
_transport.SendCommand(command);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
return FaultLeader(
|
|
"分发成员命令失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
private void ReceiveCommands(double currentTimeSeconds)
|
|
{
|
|
try
|
|
{
|
|
while (_transport.TryReceiveCommand(out var command))
|
|
{
|
|
ApplyCommand(command, currentTimeSeconds);
|
|
}
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
FaultMember(
|
|
"接收或执行主车命令失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
private void ApplyCommand(
|
|
FleetCommand command,
|
|
double currentTimeSeconds)
|
|
{
|
|
if (command.TargetVehicleId !=
|
|
FleetProtocol.BroadcastVehicleId &&
|
|
command.TargetVehicleId != SelfVehicleId)
|
|
{
|
|
return;
|
|
}
|
|
|
|
ValidateCommand(command);
|
|
var isPreparation =
|
|
command.Kind == FleetCommandKind.PrepareRolling ||
|
|
command.Kind == FleetCommandKind.PrepareSpin;
|
|
|
|
if (isPreparation && command.PlanId != CurrentPlanId)
|
|
{
|
|
if (State == FleetRuntimeState.Preparing ||
|
|
State == FleetRuntimeState.Ready ||
|
|
State == FleetRuntimeState.Active)
|
|
{
|
|
return;
|
|
}
|
|
|
|
CurrentPlanId = command.PlanId;
|
|
_lastReceivedCommandSequence = 0;
|
|
_lastAppliedCommandSequence = 0;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
if (command.PlanId != CurrentPlanId ||
|
|
command.SequenceNumber <= _lastReceivedCommandSequence)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_lastReceivedCommandSequence = command.SequenceNumber;
|
|
bool accepted;
|
|
switch (command.Kind)
|
|
{
|
|
case FleetCommandKind.PrepareRolling:
|
|
accepted = _localAgent.BeginRollingPreparation(
|
|
command.PlanId,
|
|
command.MotionDirectionInBodyRadians);
|
|
break;
|
|
case FleetCommandKind.PrepareSpin:
|
|
accepted =
|
|
_localAgent.BeginSpinPreparation(command.PlanId);
|
|
break;
|
|
case FleetCommandKind.Activate:
|
|
accepted = _localAgent.Activate(
|
|
command.PlanId,
|
|
currentTimeSeconds,
|
|
command.ValidForSeconds);
|
|
break;
|
|
case FleetCommandKind.Motion:
|
|
accepted = _localAgent.Execute(
|
|
command.PlanId,
|
|
new FleetMemberCommand(
|
|
SelfVehicleId,
|
|
command.TwistInVehicleBody),
|
|
currentTimeSeconds,
|
|
command.ValidForSeconds);
|
|
break;
|
|
case FleetCommandKind.Stop:
|
|
_localAgent.Stop();
|
|
_lastAppliedCommandSequence = command.SequenceNumber;
|
|
CurrentPlanId = FleetProtocol.NoActivePlanId;
|
|
State = FleetRuntimeState.Idle;
|
|
LastFailureReason = string.Empty;
|
|
return;
|
|
default:
|
|
accepted = false;
|
|
break;
|
|
}
|
|
|
|
if (!accepted)
|
|
{
|
|
FaultMember(LocalFailure(
|
|
$"成员车拒绝{command.Kind}命令。"));
|
|
return;
|
|
}
|
|
|
|
_lastAppliedCommandSequence = command.SequenceNumber;
|
|
State = ToRuntimeState(_localAgent.State);
|
|
}
|
|
|
|
private void SendLocalReport(
|
|
VehicleState? state,
|
|
bool isStateAvailable,
|
|
double currentTimeSeconds)
|
|
{
|
|
try
|
|
{
|
|
var value = state ?? _lastLocalState ??
|
|
new VehicleState(
|
|
currentTimeSeconds,
|
|
Pose2D.Identity,
|
|
Twist2D.Zero,
|
|
hasValidVelocityEstimate: false);
|
|
_transport.SendReport(
|
|
CreateReport(value, isStateAvailable));
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
FaultMember(
|
|
"发送成员报告失败：" + exception.Message);
|
|
}
|
|
}
|
|
|
|
private FleetMemberReport CreateReport(
|
|
VehicleState state,
|
|
bool isStateAvailable)
|
|
{
|
|
return new FleetMemberReport(
|
|
SelfVehicleId,
|
|
CurrentPlanId,
|
|
NextReportSequence(),
|
|
state.SampleTimestampSeconds,
|
|
state.PoseInWorld,
|
|
state.TwistInWorld,
|
|
isStateAvailable,
|
|
isStateAvailable && state.HasValidVelocityEstimate,
|
|
ToProtocolState(),
|
|
_lastAppliedCommandSequence,
|
|
State == FleetRuntimeState.Faulted
|
|
? RuntimeFailureCode
|
|
: 0);
|
|
}
|
|
|
|
private List<FleetMemberStateSample> BuildMemberStates()
|
|
{
|
|
var states = new List<FleetMemberStateSample>(
|
|
_activeLayout.VehicleCount);
|
|
foreach (var vehicle in _activeLayout.Vehicles)
|
|
{
|
|
var report = _reports[vehicle.VehicleId].Report;
|
|
states.Add(new FleetMemberStateSample(
|
|
report.VehicleId,
|
|
report.SampleTimestampSeconds,
|
|
report.PoseInCommonWorld,
|
|
report.TwistAtVehicleOriginInCommonWorld,
|
|
report.IsStateAvailable,
|
|
report.HasValidVelocityEstimate));
|
|
}
|
|
|
|
return states;
|
|
}
|
|
|
|
private FleetCommand NewCommand(
|
|
int targetVehicleId,
|
|
FleetCommandKind kind,
|
|
double motionDirectionInBodyRadians,
|
|
Twist2D twist,
|
|
int reasonCode = 0)
|
|
{
|
|
return new FleetCommand(
|
|
CurrentPlanId,
|
|
NextCommandSequence(),
|
|
targetVehicleId,
|
|
kind,
|
|
motionDirectionInBodyRadians,
|
|
twist,
|
|
_commandValidForSeconds,
|
|
reasonCode);
|
|
}
|
|
|
|
private bool FaultLeader(string reason)
|
|
{
|
|
var preservedReason = string.IsNullOrWhiteSpace(reason)
|
|
? "车队运行入口发生未说明故障。"
|
|
: reason;
|
|
if (CurrentPlanId > 0)
|
|
{
|
|
BroadcastStop(FaultStopReasonCode);
|
|
}
|
|
|
|
_localAgent.Stop();
|
|
State = FleetRuntimeState.Faulted;
|
|
LastFailureReason = preservedReason;
|
|
return false;
|
|
}
|
|
|
|
private void FaultMember(string reason)
|
|
{
|
|
var preservedReason = string.IsNullOrWhiteSpace(reason)
|
|
? "成员车运行入口发生未说明故障。"
|
|
: reason;
|
|
_localAgent.Stop();
|
|
State = FleetRuntimeState.Faulted;
|
|
LastFailureReason = preservedReason;
|
|
}
|
|
|
|
private void BroadcastStop(int reasonCode)
|
|
{
|
|
try
|
|
{
|
|
_transport.SendCommand(NewCommand(
|
|
FleetProtocol.BroadcastVehicleId,
|
|
FleetCommandKind.Stop,
|
|
0.0,
|
|
Twist2D.Zero,
|
|
reasonCode));
|
|
}
|
|
catch
|
|
{
|
|
// 本车仍立即停车，失联成员由各自命令看门狗兜底。
|
|
}
|
|
}
|
|
|
|
private void ResetLeaderForNewPlan()
|
|
{
|
|
_localAgent.Stop();
|
|
_preparationCoordinator.Cancel();
|
|
_fleetCoordinator.Cancel();
|
|
_safetySupervisor.Reset();
|
|
_reports.Clear();
|
|
_lastReportSequences.Clear();
|
|
_activeLayout = null;
|
|
_lastLocalState = null;
|
|
_planStartTimeSeconds = null;
|
|
_lastReceivedCommandSequence = 0;
|
|
_lastAppliedCommandSequence = 0;
|
|
LastCoordinationOutput = null;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
private void AcceptLocalCommand(long sequenceNumber)
|
|
{
|
|
_lastReceivedCommandSequence = sequenceNumber;
|
|
_lastAppliedCommandSequence = sequenceNumber;
|
|
}
|
|
|
|
private long NextCommandSequence()
|
|
{
|
|
if (++_nextCommandSequence <= 0)
|
|
{
|
|
throw new InvalidOperationException("主车命令序号耗尽。" );
|
|
}
|
|
|
|
return _nextCommandSequence;
|
|
}
|
|
|
|
private long NextReportSequence()
|
|
{
|
|
if (++_nextReportSequence <= 0)
|
|
{
|
|
throw new InvalidOperationException("成员报告序号耗尽。" );
|
|
}
|
|
|
|
return _nextReportSequence;
|
|
}
|
|
|
|
private string LocalFailure(string fallback)
|
|
{
|
|
return string.IsNullOrWhiteSpace(_localAgent.LastFailureReason)
|
|
? fallback
|
|
: _localAgent.LastFailureReason;
|
|
}
|
|
|
|
private void EnsureLeader()
|
|
{
|
|
if (!IsLeader)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"只有主车可以启动完整车队任务。");
|
|
}
|
|
}
|
|
|
|
private FleetMemberState ToProtocolState()
|
|
{
|
|
if (State == FleetRuntimeState.Faulted)
|
|
{
|
|
return FleetMemberState.Faulted;
|
|
}
|
|
|
|
return _localAgent.State switch
|
|
{
|
|
FleetMemberAgentState.Idle => FleetMemberState.Idle,
|
|
FleetMemberAgentState.Preparing => FleetMemberState.Preparing,
|
|
FleetMemberAgentState.Ready => FleetMemberState.Ready,
|
|
FleetMemberAgentState.Active => FleetMemberState.Active,
|
|
FleetMemberAgentState.Faulted => FleetMemberState.Faulted,
|
|
_ => throw new InvalidOperationException("本车状态无效。")
|
|
};
|
|
}
|
|
|
|
private static FleetRuntimeState ToRuntimeState(
|
|
FleetMemberAgentState state)
|
|
{
|
|
return state switch
|
|
{
|
|
FleetMemberAgentState.Idle => FleetRuntimeState.Idle,
|
|
FleetMemberAgentState.Preparing => FleetRuntimeState.Preparing,
|
|
FleetMemberAgentState.Ready => FleetRuntimeState.Ready,
|
|
FleetMemberAgentState.Active => FleetRuntimeState.Active,
|
|
FleetMemberAgentState.Faulted => FleetRuntimeState.Faulted,
|
|
_ => throw new ArgumentOutOfRangeException(nameof(state))
|
|
};
|
|
}
|
|
|
|
private static FleetMemberAgentState ToAgentState(
|
|
FleetMemberState state)
|
|
{
|
|
return state switch
|
|
{
|
|
FleetMemberState.Idle => FleetMemberAgentState.Idle,
|
|
FleetMemberState.Preparing => FleetMemberAgentState.Preparing,
|
|
FleetMemberState.Ready => FleetMemberAgentState.Ready,
|
|
FleetMemberState.Active => FleetMemberAgentState.Active,
|
|
FleetMemberState.Faulted => FleetMemberAgentState.Faulted,
|
|
_ => throw new ArgumentOutOfRangeException(nameof(state))
|
|
};
|
|
}
|
|
|
|
private static void ValidateCommand(FleetCommand command)
|
|
{
|
|
if (command.PlanId <= 0 || command.SequenceNumber <= 0)
|
|
{
|
|
throw new ArgumentException("车队命令任务号或序号无效。" );
|
|
}
|
|
|
|
if (!Enum.IsDefined(typeof(FleetCommandKind), command.Kind))
|
|
{
|
|
throw new ArgumentException("车队命令类型无效。" );
|
|
}
|
|
|
|
NumericGuard.EnsureFinite(
|
|
command.MotionDirectionInBodyRadians,
|
|
nameof(command));
|
|
NumericGuard.EnsureFinite(
|
|
command.TwistInVehicleBody,
|
|
nameof(command));
|
|
NumericGuard.EnsureFinitePositive(
|
|
command.ValidForSeconds,
|
|
nameof(command));
|
|
}
|
|
|
|
private static void ValidateReport(FleetMemberReport report)
|
|
{
|
|
if (report.VehicleId <= 0 || report.PlanId <= 0 ||
|
|
report.SequenceNumber <= 0 ||
|
|
report.LastAppliedCommandSequence < 0 ||
|
|
!Enum.IsDefined(typeof(FleetMemberState), report.State))
|
|
{
|
|
throw new ArgumentException("成员状态报告字段无效。" );
|
|
}
|
|
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
report.SampleTimestampSeconds,
|
|
nameof(report));
|
|
NumericGuard.EnsureFinite(
|
|
report.PoseInCommonWorld,
|
|
nameof(report));
|
|
NumericGuard.EnsureFinite(
|
|
report.TwistAtVehicleOriginInCommonWorld,
|
|
nameof(report));
|
|
}
|
|
|
|
private readonly struct AcceptedReport
|
|
{
|
|
public AcceptedReport(
|
|
FleetMemberReport report,
|
|
double receivedTimeSeconds)
|
|
{
|
|
Report = report;
|
|
ReceivedTimeSeconds = receivedTimeSeconds;
|
|
}
|
|
|
|
public FleetMemberReport Report { get; }
|
|
|
|
public double ReceivedTimeSeconds { get; }
|
|
}
|
|
}
|
|
}
|