570 lines
19 KiB
C#
570 lines
19 KiB
C#
using System;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC.Fleet
|
||
{
|
||
/// <summary>表示成员车在一次车队动作中的本地执行阶段。</summary>
|
||
public enum FleetMemberAgentState
|
||
{
|
||
Idle = 0,
|
||
Preparing = 1,
|
||
Ready = 2,
|
||
Active = 3,
|
||
Faulted = 4
|
||
}
|
||
|
||
/// <summary>区分固定β滚动运动和车辆中心纯自转的准备方式。</summary>
|
||
public enum FleetMemberPreparationMode
|
||
{
|
||
Rolling = 0,
|
||
Spin = 1
|
||
}
|
||
|
||
/// <summary>负责一辆成员车的舵轮准备、激活和本车速度命令执行。</summary>
|
||
public sealed class FleetMemberAgent
|
||
{
|
||
private const double MotionDeadband = 1e-6;
|
||
|
||
private readonly MultiWheelChassisAdapter _adapter;
|
||
private readonly double _alignmentToleranceRadians;
|
||
private readonly double _alignmentStableSeconds;
|
||
|
||
private double _alignedDurationSeconds;
|
||
private double? _lastAcceptedCommandTimeSeconds;
|
||
private double? _commandDeadlineSeconds;
|
||
|
||
/// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
|
||
public FleetMemberAgent(
|
||
MultiWheelChassisAdapter adapter,
|
||
double alignmentToleranceRadians,
|
||
double alignmentStableSeconds)
|
||
{
|
||
_adapter = adapter ??
|
||
throw new ArgumentNullException(nameof(adapter));
|
||
NumericGuard.EnsureFinitePositive(
|
||
alignmentToleranceRadians,
|
||
nameof(alignmentToleranceRadians));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
alignmentStableSeconds,
|
||
nameof(alignmentStableSeconds));
|
||
|
||
if (alignmentToleranceRadians > Math.PI)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(alignmentToleranceRadians),
|
||
"舵轮到位容差不能大于π。");
|
||
}
|
||
|
||
_alignmentToleranceRadians =
|
||
alignmentToleranceRadians;
|
||
_alignmentStableSeconds =
|
||
alignmentStableSeconds;
|
||
State = FleetMemberAgentState.Idle;
|
||
LastFailureReason = string.Empty;
|
||
}
|
||
|
||
public int VehicleId => _adapter.VehicleId;
|
||
|
||
public FleetMemberAgentState State { get; private set; }
|
||
|
||
public FleetMemberPreparationMode? PreparationMode
|
||
{
|
||
get;
|
||
private set;
|
||
}
|
||
|
||
public long CurrentPlanId { get; private set; }
|
||
|
||
public double MotionDirectionInBodyRadians
|
||
{
|
||
get;
|
||
private set;
|
||
}
|
||
|
||
public string LastFailureReason { get; private set; }
|
||
|
||
// 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。
|
||
public double? LastAcceptedCommandTimeSeconds =>
|
||
_lastAcceptedCommandTimeSeconds;
|
||
|
||
public double? CommandDeadlineSeconds =>
|
||
_commandDeadlineSeconds;
|
||
|
||
/// <summary>停车并开始准备本车固定β滚动运动系。</summary>
|
||
public bool BeginRollingPreparation(
|
||
long planId,
|
||
double motionDirectionInBodyRadians)
|
||
{
|
||
ValidatePlanId(planId);
|
||
NumericGuard.EnsureFinite(
|
||
motionDirectionInBodyRadians,
|
||
nameof(motionDirectionInBodyRadians));
|
||
|
||
return BeginPreparation(
|
||
planId,
|
||
FleetMemberPreparationMode.Rolling,
|
||
AngleMath.NormalizeRadians(
|
||
motionDirectionInBodyRadians));
|
||
}
|
||
|
||
/// <summary>停车并开始准备车辆中心纯自转所需的舵轮方向。</summary>
|
||
public bool BeginSpinPreparation(long planId)
|
||
{
|
||
ValidatePlanId(planId);
|
||
return BeginPreparation(
|
||
planId,
|
||
FleetMemberPreparationMode.Spin,
|
||
motionDirectionInBodyRadians: 0.0);
|
||
}
|
||
|
||
/// <summary>检查舵轮是否已连续稳定到位；宿主应在准备阶段周期调用。</summary>
|
||
public FleetMemberAgentState UpdatePreparation(
|
||
double deltaTimeSeconds)
|
||
{
|
||
NumericGuard.EnsureFinitePositive(
|
||
deltaTimeSeconds,
|
||
nameof(deltaTimeSeconds));
|
||
|
||
if (State != FleetMemberAgentState.Preparing)
|
||
{
|
||
return State;
|
||
}
|
||
|
||
bool aligned;
|
||
try
|
||
{
|
||
aligned = UpdateAndCheckAlignment();
|
||
}
|
||
catch (InvalidOperationException exception)
|
||
{
|
||
Fail(exception.Message);
|
||
return State;
|
||
}
|
||
catch (ArgumentException exception)
|
||
{
|
||
Fail(exception.Message);
|
||
return State;
|
||
}
|
||
|
||
if (State == FleetMemberAgentState.Faulted)
|
||
{
|
||
return State;
|
||
}
|
||
|
||
_alignedDurationSeconds = aligned
|
||
? _alignedDurationSeconds + deltaTimeSeconds
|
||
: 0.0;
|
||
|
||
if (aligned &&
|
||
_alignedDurationSeconds >=
|
||
_alignmentStableSeconds)
|
||
{
|
||
State = FleetMemberAgentState.Ready;
|
||
LastFailureReason = string.Empty;
|
||
}
|
||
|
||
return State;
|
||
}
|
||
|
||
/// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
|
||
public bool Activate(
|
||
long planId,
|
||
double commandReceivedTimeSeconds,
|
||
double validForSeconds)
|
||
{
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
commandReceivedTimeSeconds,
|
||
nameof(commandReceivedTimeSeconds));
|
||
NumericGuard.EnsureFinitePositive(
|
||
validForSeconds,
|
||
nameof(validForSeconds));
|
||
|
||
if (planId != CurrentPlanId)
|
||
{
|
||
LastFailureReason =
|
||
"激活任务编号与当前准备任务不一致。";
|
||
return false;
|
||
}
|
||
|
||
if (State == FleetMemberAgentState.Active)
|
||
{
|
||
// 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。
|
||
return UpdateCommandWatchdog(
|
||
commandReceivedTimeSeconds);
|
||
}
|
||
|
||
if (State != FleetMemberAgentState.Ready ||
|
||
!PreparationMode.HasValue)
|
||
{
|
||
return RejectWhileStopped(
|
||
"成员车尚未完成舵轮准备。");
|
||
}
|
||
|
||
bool stillAligned;
|
||
try
|
||
{
|
||
stillAligned = ArePreparedWheelsStillAligned();
|
||
}
|
||
catch (InvalidOperationException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
catch (ArgumentException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
|
||
if (!stillAligned)
|
||
{
|
||
State = FleetMemberAgentState.Preparing;
|
||
_alignedDurationSeconds = 0.0;
|
||
return RejectWhileStopped(
|
||
"成员车在激活前失去舵轮到位状态。");
|
||
}
|
||
|
||
try
|
||
{
|
||
if (PreparationMode.Value ==
|
||
FleetMemberPreparationMode.Rolling)
|
||
{
|
||
_adapter.ActivateMotionFrame(
|
||
MotionDirectionInBodyRadians);
|
||
}
|
||
else if (!_adapter.AdoptPreparedSpinForXYTh(
|
||
_alignmentToleranceRadians))
|
||
{
|
||
return Fail(
|
||
BuildAdapterFailureReason(
|
||
"无法激活已经准备好的原地自转舵轮。"));
|
||
}
|
||
}
|
||
catch (InvalidOperationException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
catch (ArgumentException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
|
||
State = FleetMemberAgentState.Active;
|
||
AcceptCommandDeadline(
|
||
commandReceivedTimeSeconds,
|
||
validForSeconds);
|
||
LastFailureReason = string.Empty;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>校验任务和车号后执行分配给本车的车体系速度命令。</summary>
|
||
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)
|
||
{
|
||
return Fail(
|
||
"速度命令任务编号与当前激活任务不一致。");
|
||
}
|
||
|
||
if (command.VehicleId != VehicleId)
|
||
{
|
||
return Fail(
|
||
$"速度命令属于车辆{command.VehicleId}，" +
|
||
$"当前成员车号为{VehicleId}。");
|
||
}
|
||
|
||
if (State != FleetMemberAgentState.Active ||
|
||
!PreparationMode.HasValue)
|
||
{
|
||
return RejectWhileStopped(
|
||
"成员车尚未激活，不能执行速度命令。");
|
||
}
|
||
|
||
// 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。
|
||
if (!UpdateCommandWatchdog(
|
||
commandReceivedTimeSeconds))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!IsCommandCompatibleWithPreparation(
|
||
command.TwistInVehicleBody))
|
||
{
|
||
return Fail(
|
||
"速度命令与本次舵轮准备方式不一致。");
|
||
}
|
||
|
||
try
|
||
{
|
||
if (!_adapter.SendBodyTwist(
|
||
command.TwistInVehicleBody,
|
||
interval))
|
||
{
|
||
return Fail(
|
||
BuildAdapterFailureReason(
|
||
"成员车底盘拒绝执行速度命令。"));
|
||
}
|
||
}
|
||
catch (InvalidOperationException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
catch (ArgumentException exception)
|
||
{
|
||
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()
|
||
{
|
||
_adapter.StopImmediately();
|
||
State = FleetMemberAgentState.Idle;
|
||
PreparationMode = null;
|
||
CurrentPlanId = 0;
|
||
MotionDirectionInBodyRadians = 0.0;
|
||
_alignedDurationSeconds = 0.0;
|
||
ClearCommandWatchdog();
|
||
LastFailureReason = string.Empty;
|
||
}
|
||
|
||
/// <summary>重置上一动作并下发本次滚动或自转舵轮准备目标。</summary>
|
||
private bool BeginPreparation(
|
||
long planId,
|
||
FleetMemberPreparationMode mode,
|
||
double motionDirectionInBodyRadians)
|
||
{
|
||
try
|
||
{
|
||
_adapter.StopImmediately();
|
||
_adapter.ResetToBodyFrame();
|
||
|
||
CurrentPlanId = planId;
|
||
PreparationMode = mode;
|
||
MotionDirectionInBodyRadians =
|
||
motionDirectionInBodyRadians;
|
||
State = FleetMemberAgentState.Preparing;
|
||
LastFailureReason = string.Empty;
|
||
_alignedDurationSeconds = 0.0;
|
||
ClearCommandWatchdog();
|
||
|
||
var accepted = mode ==
|
||
FleetMemberPreparationMode.Rolling
|
||
? _adapter.PrepareParallelDirection(
|
||
motionDirectionInBodyRadians)
|
||
: _adapter.PrepareSpin(
|
||
alignmentToleranceDegrees:
|
||
AngleMath.RadiansToDegrees(
|
||
_alignmentToleranceRadians));
|
||
|
||
if (!accepted)
|
||
{
|
||
return Fail(
|
||
BuildAdapterFailureReason(
|
||
"成员车底盘拒绝舵轮准备目标。"));
|
||
}
|
||
|
||
return true;
|
||
}
|
||
catch (InvalidOperationException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
catch (ArgumentException exception)
|
||
{
|
||
return Fail(exception.Message);
|
||
}
|
||
}
|
||
|
||
/// <summary>更新当前准备目标并读取舵轮到位状态。</summary>
|
||
private bool UpdateAndCheckAlignment()
|
||
{
|
||
if (PreparationMode ==
|
||
FleetMemberPreparationMode.Rolling)
|
||
{
|
||
return _adapter.AreParallelWheelsAligned(
|
||
MotionDirectionInBodyRadians,
|
||
_alignmentToleranceRadians);
|
||
}
|
||
|
||
if (!_adapter.PrepareSpin(
|
||
alignmentToleranceDegrees:
|
||
AngleMath.RadiansToDegrees(
|
||
_alignmentToleranceRadians)))
|
||
{
|
||
Fail(
|
||
BuildAdapterFailureReason(
|
||
"成员车底盘无法继续更新原地自转准备。"));
|
||
return false;
|
||
}
|
||
|
||
return _adapter.AreSpinWheelsAligned;
|
||
}
|
||
|
||
/// <summary>确认舵轮在全队释放前仍保持到位。</summary>
|
||
private bool ArePreparedWheelsStillAligned()
|
||
{
|
||
return PreparationMode ==
|
||
FleetMemberPreparationMode.Rolling
|
||
? _adapter.AreParallelWheelsAligned(
|
||
MotionDirectionInBodyRadians,
|
||
_alignmentToleranceRadians)
|
||
: _adapter.AreSpinWheelsAligned;
|
||
}
|
||
|
||
/// <summary>禁止滚动准备执行纯自转，也禁止自转准备执行平移。</summary>
|
||
private bool IsCommandCompatibleWithPreparation(
|
||
Twist2D bodyTwist)
|
||
{
|
||
var linearSpeed = Math.Sqrt(
|
||
bodyTwist.VxMetersPerSecond *
|
||
bodyTwist.VxMetersPerSecond +
|
||
bodyTwist.VyMetersPerSecond *
|
||
bodyTwist.VyMetersPerSecond);
|
||
var hasLinearMotion =
|
||
linearSpeed > MotionDeadband;
|
||
var hasAngularMotion =
|
||
Math.Abs(
|
||
bodyTwist.OmegaRadiansPerSecond) >
|
||
MotionDeadband;
|
||
|
||
if (!hasLinearMotion && !hasAngularMotion)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return PreparationMode ==
|
||
FleetMemberPreparationMode.Rolling
|
||
? hasLinearMotion
|
||
: !hasLinearMotion && hasAngularMotion;
|
||
}
|
||
|
||
/// <summary>拒绝未满足执行条件的命令并保持车辆零速。</summary>
|
||
private bool RejectWhileStopped(string reason)
|
||
{
|
||
_adapter.StopImmediately();
|
||
LastFailureReason = reason ?? string.Empty;
|
||
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)
|
||
{
|
||
_adapter.StopImmediately();
|
||
State = FleetMemberAgentState.Faulted;
|
||
LastFailureReason = reason ?? string.Empty;
|
||
return false;
|
||
}
|
||
|
||
/// <summary>优先返回底盘提供的具体失败原因。</summary>
|
||
private string BuildAdapterFailureReason(
|
||
string fallbackReason)
|
||
{
|
||
return string.IsNullOrWhiteSpace(
|
||
_adapter.LastFailureReason)
|
||
? fallbackReason
|
||
: _adapter.LastFailureReason;
|
||
}
|
||
|
||
/// <summary>拒绝零值和负值任务编号。</summary>
|
||
private static void ValidatePlanId(long planId)
|
||
{
|
||
if (planId <= 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(planId),
|
||
"车队动作任务编号必须大于零。");
|
||
}
|
||
}
|
||
}
|
||
}
|