374 lines
12 KiB
C#
374 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MyParking.Shared;
|
|
// 负责运动前：所有车辆舵轮是否准备完成
|
|
|
|
namespace MultiWheelC.Fleet
|
|
{
|
|
/// <summary>表示主车侧车队运动准备的当前阶段。</summary>
|
|
public enum FleetPreparationCoordinatorState
|
|
{
|
|
Idle = 0,
|
|
WaitingForMembers = 1,
|
|
ReadyToActivate = 2,
|
|
ActivationAuthorized = 3,
|
|
Faulted = 4
|
|
}
|
|
|
|
/// <summary>保存一次滚动准备中分配给指定成员车的本地β目标。</summary>
|
|
public readonly struct FleetMemberPreparationTarget
|
|
{
|
|
/// <summary>创建一条属于指定任务和成员车的准备目标。</summary>
|
|
public FleetMemberPreparationTarget(
|
|
long planId,
|
|
int vehicleId,
|
|
double motionDirectionInBodyRadians)
|
|
{
|
|
if (planId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(planId),
|
|
"车队动作任务编号必须大于零。");
|
|
}
|
|
|
|
if (vehicleId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(vehicleId),
|
|
"成员车号必须大于零。");
|
|
}
|
|
|
|
NumericGuard.EnsureFinite(
|
|
motionDirectionInBodyRadians,
|
|
nameof(motionDirectionInBodyRadians));
|
|
|
|
PlanId = planId;
|
|
VehicleId = vehicleId;
|
|
MotionDirectionInBodyRadians =
|
|
AngleMath.NormalizeRadians(
|
|
motionDirectionInBodyRadians);
|
|
}
|
|
|
|
public long PlanId { get; }
|
|
|
|
public int VehicleId { get; }
|
|
|
|
public double MotionDirectionInBodyRadians { get; }
|
|
}
|
|
|
|
/// <summary>保存成员车对某次准备任务上报的本地状态。</summary>
|
|
public readonly struct FleetMemberPreparationStatus
|
|
{
|
|
/// <summary>创建一条成员车准备状态报告。</summary>
|
|
public FleetMemberPreparationStatus(
|
|
long planId,
|
|
int vehicleId,
|
|
FleetMemberAgentState state,
|
|
string failureReason = "")
|
|
{
|
|
if (planId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(planId),
|
|
"车队动作任务编号必须大于零。");
|
|
}
|
|
|
|
if (vehicleId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(vehicleId),
|
|
"成员车号必须大于零。");
|
|
}
|
|
|
|
if (!Enum.IsDefined(
|
|
typeof(FleetMemberAgentState),
|
|
state))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(state),
|
|
"成员车准备状态无效。");
|
|
}
|
|
|
|
PlanId = planId;
|
|
VehicleId = vehicleId;
|
|
State = state;
|
|
FailureReason = failureReason ?? string.Empty;
|
|
}
|
|
|
|
public long PlanId { get; }
|
|
|
|
public int VehicleId { get; }
|
|
|
|
public FleetMemberAgentState State { get; }
|
|
|
|
public string FailureReason { get; }
|
|
}
|
|
|
|
/// <summary>在主车侧分配成员β并管理全队Ready统一激活屏障。</summary>
|
|
public sealed class FleetPreparationCoordinator
|
|
{
|
|
private static readonly IReadOnlyList<
|
|
FleetMemberPreparationTarget>
|
|
EmptyTargets = Array.AsReadOnly(
|
|
Array.Empty<FleetMemberPreparationTarget>());
|
|
|
|
private readonly Dictionary<int, FleetMemberAgentState>
|
|
_memberStates =
|
|
new Dictionary<int, FleetMemberAgentState>();
|
|
|
|
private IReadOnlyList<FleetMemberPreparationTarget>
|
|
_targets = EmptyTargets;
|
|
|
|
/// <summary>创建尚未激活准备任务的主车侧协调器。</summary>
|
|
public FleetPreparationCoordinator()
|
|
{
|
|
State = FleetPreparationCoordinatorState.Idle;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
public FleetPreparationCoordinatorState State
|
|
{
|
|
get;
|
|
private set;
|
|
}
|
|
|
|
public long CurrentPlanId { get; private set; }
|
|
|
|
public double MotionDirectionInFleetRadians
|
|
{
|
|
get;
|
|
private set;
|
|
}
|
|
|
|
public IReadOnlyList<FleetMemberPreparationTarget>
|
|
Targets => _targets;
|
|
|
|
public string LastFailureReason { get; private set; }
|
|
|
|
/// <summary>根据车队固定布局为全部成员建立本次滚动β准备目标。</summary>
|
|
public void StartRollingPreparation(
|
|
long planId,
|
|
FleetLayout layout,
|
|
double motionDirectionInFleetRadians)
|
|
{
|
|
if (planId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(planId),
|
|
"车队动作任务编号必须大于零。");
|
|
}
|
|
|
|
if (layout == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(layout));
|
|
}
|
|
|
|
NumericGuard.EnsureFinite(
|
|
motionDirectionInFleetRadians,
|
|
nameof(motionDirectionInFleetRadians));
|
|
|
|
var normalizedFleetDirection =
|
|
AngleMath.NormalizeRadians(
|
|
motionDirectionInFleetRadians);
|
|
var targets =
|
|
new FleetMemberPreparationTarget[
|
|
layout.VehicleCount];
|
|
|
|
_memberStates.Clear();
|
|
for (var index = 0;
|
|
index < layout.Vehicles.Count;
|
|
index++)
|
|
{
|
|
var vehicle = layout.Vehicles[index];
|
|
var rawDirectionInBody =
|
|
AngleMath.NormalizeRadians(
|
|
normalizedFleetDirection -
|
|
vehicle.PoseInFleet.YawRadians);
|
|
var equivalentDirectionInBody =
|
|
SelectSteeringAxisEquivalent(
|
|
rawDirectionInBody);
|
|
|
|
targets[index] =
|
|
new FleetMemberPreparationTarget(
|
|
planId,
|
|
vehicle.VehicleId,
|
|
equivalentDirectionInBody);
|
|
_memberStates.Add(
|
|
vehicle.VehicleId,
|
|
FleetMemberAgentState.Idle);
|
|
}
|
|
|
|
CurrentPlanId = planId;
|
|
MotionDirectionInFleetRadians =
|
|
normalizedFleetDirection;
|
|
_targets = Array.AsReadOnly(targets);
|
|
State =
|
|
FleetPreparationCoordinatorState
|
|
.WaitingForMembers;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
/// <summary>接收一辆成员车的状态并重新计算全队Ready状态。</summary>
|
|
public FleetPreparationCoordinatorState
|
|
ReportMemberStatus(
|
|
FleetMemberPreparationStatus status)
|
|
{
|
|
if (State ==
|
|
FleetPreparationCoordinatorState.Idle ||
|
|
State ==
|
|
FleetPreparationCoordinatorState.Faulted)
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (status.PlanId != CurrentPlanId)
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (!_memberStates.ContainsKey(status.VehicleId))
|
|
{
|
|
throw new ArgumentException(
|
|
$"车辆{status.VehicleId}不属于当前车队布局。",
|
|
nameof(status));
|
|
}
|
|
|
|
if (status.State ==
|
|
FleetMemberAgentState.Faulted)
|
|
{
|
|
return Fail(
|
|
string.IsNullOrWhiteSpace(
|
|
status.FailureReason)
|
|
? $"车辆{status.VehicleId}准备失败。"
|
|
: $"车辆{status.VehicleId}准备失败：" +
|
|
status.FailureReason);
|
|
}
|
|
|
|
if (State ==
|
|
FleetPreparationCoordinatorState
|
|
.ActivationAuthorized)
|
|
{
|
|
return State;
|
|
}
|
|
|
|
if (status.State ==
|
|
FleetMemberAgentState.Active)
|
|
{
|
|
return Fail(
|
|
$"车辆{status.VehicleId}在全队统一激活前已经进入Active。");
|
|
}
|
|
|
|
_memberStates[status.VehicleId] = status.State;
|
|
State = AreAllMembersReady()
|
|
? FleetPreparationCoordinatorState
|
|
.ReadyToActivate
|
|
: FleetPreparationCoordinatorState
|
|
.WaitingForMembers;
|
|
LastFailureReason = string.Empty;
|
|
return State;
|
|
}
|
|
|
|
/// <summary>在全部成员Ready后授权外层向全队广播同一任务的激活命令。</summary>
|
|
public bool TryAuthorizeActivation(long planId)
|
|
{
|
|
if (planId != CurrentPlanId)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (State ==
|
|
FleetPreparationCoordinatorState
|
|
.ActivationAuthorized)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if (State !=
|
|
FleetPreparationCoordinatorState
|
|
.ReadyToActivate)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
State = FleetPreparationCoordinatorState
|
|
.ActivationAuthorized;
|
|
LastFailureReason = string.Empty;
|
|
return true;
|
|
}
|
|
|
|
/// <summary>查找指定成员车在当前任务中的本地β准备目标。</summary>
|
|
public bool TryGetTarget(
|
|
int vehicleId,
|
|
out FleetMemberPreparationTarget target)
|
|
{
|
|
for (var index = 0;
|
|
index < _targets.Count;
|
|
index++)
|
|
{
|
|
if (_targets[index].VehicleId == vehicleId)
|
|
{
|
|
target = _targets[index];
|
|
return true;
|
|
}
|
|
}
|
|
|
|
target = default;
|
|
return false;
|
|
}
|
|
|
|
/// <summary>取消当前准备任务并清除成员状态和β目标。</summary>
|
|
public void Cancel()
|
|
{
|
|
_memberStates.Clear();
|
|
_targets = EmptyTargets;
|
|
CurrentPlanId = 0;
|
|
MotionDirectionInFleetRadians = 0.0;
|
|
State = FleetPreparationCoordinatorState.Idle;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
/// <summary>判断当前任务中的每辆成员车是否都已报告Ready。</summary>
|
|
private bool AreAllMembersReady()
|
|
{
|
|
foreach (var state in _memberStates.Values)
|
|
{
|
|
if (state != FleetMemberAgentState.Ready)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return _memberStates.Count > 0;
|
|
}
|
|
|
|
/// <summary>将有向β转换为±90°内的等效滚动轴，反向运动由轮速符号表达。</summary>
|
|
private static double SelectSteeringAxisEquivalent(
|
|
double directionRadians)
|
|
{
|
|
var equivalent = AngleMath.NormalizeRadians(
|
|
directionRadians);
|
|
|
|
if (equivalent > Math.PI / 2.0)
|
|
{
|
|
equivalent -= Math.PI;
|
|
}
|
|
else if (equivalent < -Math.PI / 2.0)
|
|
{
|
|
equivalent += Math.PI;
|
|
}
|
|
|
|
return AngleMath.NormalizeRadians(equivalent);
|
|
}
|
|
|
|
/// <summary>锁存准备故障，等待外层停止所有成员并取消任务。</summary>
|
|
private FleetPreparationCoordinatorState Fail(
|
|
string reason)
|
|
{
|
|
State = FleetPreparationCoordinatorState.Faulted;
|
|
LastFailureReason = reason ?? string.Empty;
|
|
return State;
|
|
}
|
|
}
|
|
}
|