完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -1 +1,456 @@
|
||||
// 主车、从车都有，负责执行分配给本车的命令
|
||||
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;
|
||||
|
||||
/// <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; }
|
||||
|
||||
/// <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)
|
||||
{
|
||||
if (planId != CurrentPlanId)
|
||||
{
|
||||
LastFailureReason =
|
||||
"激活任务编号与当前准备任务不一致。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (State == FleetMemberAgentState.Active)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
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;
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>校验任务和车号后执行分配给本车的车体系速度命令。</summary>
|
||||
public bool Execute(
|
||||
long planId,
|
||||
FleetMemberCommand command,
|
||||
TimeSpan? interval = null)
|
||||
{
|
||||
ValidatePlanId(planId);
|
||||
NumericGuard.EnsureFinite(
|
||||
command.TwistInVehicleBody,
|
||||
nameof(command));
|
||||
|
||||
if (planId != CurrentPlanId)
|
||||
{
|
||||
return Fail(
|
||||
"速度命令任务编号与当前激活任务不一致。");
|
||||
}
|
||||
|
||||
if (command.VehicleId != VehicleId)
|
||||
{
|
||||
return Fail(
|
||||
$"速度命令属于车辆{command.VehicleId}，" +
|
||||
$"当前成员车号为{VehicleId}。");
|
||||
}
|
||||
|
||||
if (State != FleetMemberAgentState.Active ||
|
||||
!PreparationMode.HasValue)
|
||||
{
|
||||
return RejectWhileStopped(
|
||||
"成员车尚未激活，不能执行速度命令。");
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>正常取消当前任务并立即停止驱动轮。</summary>
|
||||
public void Stop()
|
||||
{
|
||||
_adapter.StopImmediately();
|
||||
State = FleetMemberAgentState.Idle;
|
||||
PreparationMode = null;
|
||||
CurrentPlanId = 0;
|
||||
MotionDirectionInBodyRadians = 0.0;
|
||||
_alignedDurationSeconds = 0.0;
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/// <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),
|
||||
"车队动作任务编号必须大于零。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user