完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -0,0 +1,372 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user