97 lines
5.3 KiB
C#
97 lines
5.3 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>按调用方时刻采样不可变 EM 轨迹，并应用纯零速换向状态机；从不在轨迹末点之后外推。</summary>
|
||
public sealed class TrajectoryExecutor
|
||
{
|
||
private readonly TrajectorySampler sampler = new TrajectorySampler();
|
||
private readonly TrajectoryControlAdapter controlAdapter = new TrajectoryControlAdapter();
|
||
private readonly GearSwitchStateMachine gearSwitchStateMachine;
|
||
|
||
/// <summary>使用默认 EM 配置创建执行器，默认停车容差和零速 dwell 来自该配置。</summary>
|
||
public TrajectoryExecutor()
|
||
: this(EmPlannerConfiguration.CreateDefault())
|
||
{
|
||
}
|
||
|
||
/// <summary>使用指定 EM 配置创建执行器。</summary>
|
||
/// <param name="configuration">必须提供纵向停车速度容差（m/s）和零速 dwell（s）的配置快照。</param>
|
||
public TrajectoryExecutor(EmPlannerConfiguration configuration)
|
||
{
|
||
if (configuration?.Longitudinal == null)
|
||
throw new ArgumentNullException(nameof(configuration));
|
||
gearSwitchStateMachine = new GearSwitchStateMachine(
|
||
configuration.Longitudinal.StopSpeedToleranceMetersPerSecond,
|
||
configuration.Longitudinal.ZeroSpeedHoldSeconds);
|
||
}
|
||
|
||
/// <summary>最近一次更新产生的不可变执行状态；尚未调用 <see cref="Update"/> 时为 <see langword="null"/>。</summary>
|
||
public TrajectoryExecutionState State { get; private set; }
|
||
|
||
/// <summary>按调用方时刻选择轨迹点并更新换向状态机。</summary>
|
||
/// <param name="now">调用方当前时刻；相对轨迹生效时间换算为采样秒数。</param>
|
||
/// <param name="measuredState">车辆测量状态快照；带符号纵向速度单位 m/s，用于判断是否已停稳。</param>
|
||
/// <param name="trajectory">已发布的完整不可变轨迹；开始前取首点、结束后取末点且不外推。</param>
|
||
/// <param name="desiredDirection">下一方向段要求的实际行驶方向。</param>
|
||
/// <param name="currentDirection">调用方确认的当前实际方向。</param>
|
||
/// <param name="directionConfirmed">硬件/调用方是否已确认方向变更完成。</param>
|
||
/// <returns>包含选中点、换向状态、停零、换向请求和完成标记的不可变执行状态。</returns>
|
||
public TrajectoryExecutionState Update(DateTimeOffset now, VehicleMotionState measuredState, EmTrajectory trajectory,
|
||
TravelDirection desiredDirection, TravelDirection currentDirection, bool directionConfirmed)
|
||
{
|
||
if (measuredState == null)
|
||
throw new ArgumentNullException(nameof(measuredState));
|
||
if (trajectory == null)
|
||
throw new ArgumentNullException(nameof(trajectory));
|
||
|
||
EmTrajectoryPoint selectedPoint = SelectPoint(trajectory, now);
|
||
bool atGearSwitchBoundary = selectedPoint.BoundaryType == EmBoundaryType.GearSwitchApproach;
|
||
bool atTerminal = selectedPoint.BoundaryType == EmBoundaryType.Goal ||
|
||
selectedPoint.BoundaryType == EmBoundaryType.RollingSafetyStop;
|
||
GearSwitchStateUpdate update = gearSwitchStateMachine.Update(now,
|
||
measuredState.SignedLongitudinalSpeedMetersPerSecond, desiredDirection, currentDirection,
|
||
directionConfirmed, atGearSwitchBoundary, atTerminal);
|
||
State = new TrajectoryExecutionState(selectedPoint, update);
|
||
return State;
|
||
}
|
||
|
||
/// <summary>更新纯执行状态并返回控制器中立命令。</summary>
|
||
/// <param name="now">调用方当前时刻。</param>
|
||
/// <param name="measuredState">车辆测量状态快照。</param>
|
||
/// <param name="trajectory">已发布完整轨迹。</param>
|
||
/// <param name="desiredDirection">目标实际行驶方向。</param>
|
||
/// <param name="currentDirection">已确认当前方向。</param>
|
||
/// <param name="directionConfirmed">是否已确认换向。</param>
|
||
/// <returns>常规跟随时输出轨迹 m/s 与 rad/s；停零、等待确认或完成时输出零运动和制动。</returns>
|
||
public TrajectoryControlCommand UpdateCommand(DateTimeOffset now, VehicleMotionState measuredState,
|
||
EmTrajectory trajectory, TravelDirection desiredDirection, TravelDirection currentDirection,
|
||
bool directionConfirmed)
|
||
{
|
||
TrajectoryExecutionState state = Update(now, measuredState, trajectory, desiredDirection, currentDirection,
|
||
directionConfirmed);
|
||
return controlAdapter.CreateCommand(state.SelectedPoint, state);
|
||
}
|
||
|
||
private EmTrajectoryPoint SelectPoint(EmTrajectory trajectory, DateTimeOffset now)
|
||
{
|
||
double timeFromStart = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds;
|
||
EmTrajectoryPoint first = trajectory.Points[0];
|
||
EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1];
|
||
if (timeFromStart <= first.TimeFromStart)
|
||
return first;
|
||
if (timeFromStart >= last.TimeFromStart)
|
||
return last;
|
||
if (sampler.TrySample(trajectory, timeFromStart, out EmTrajectoryPoint sampled))
|
||
return sampled;
|
||
|
||
for (int index = trajectory.Points.Count - 1; index >= 0; index--)
|
||
{
|
||
if (trajectory.Points[index].TimeFromStart <= timeFromStart)
|
||
return trajectory.Points[index];
|
||
}
|
||
return first;
|
||
}
|
||
}
|