using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
/// 按调用方时刻采样不可变 EM 轨迹，并应用纯零速换向状态机；从不在轨迹末点之后外推。
public sealed class TrajectoryExecutor
{
private readonly TrajectorySampler sampler = new TrajectorySampler();
private readonly TrajectoryControlAdapter controlAdapter = new TrajectoryControlAdapter();
private readonly GearSwitchStateMachine gearSwitchStateMachine;
/// 使用默认 EM 配置创建执行器，默认停车容差和零速 dwell 来自该配置。
public TrajectoryExecutor()
: this(EmPlannerConfiguration.CreateDefault())
{
}
/// 使用指定 EM 配置创建执行器。
/// 必须提供纵向停车速度容差（m/s）和零速 dwell（s）的配置快照。
public TrajectoryExecutor(EmPlannerConfiguration configuration)
{
if (configuration?.Longitudinal == null)
throw new ArgumentNullException(nameof(configuration));
gearSwitchStateMachine = new GearSwitchStateMachine(
configuration.Longitudinal.StopSpeedToleranceMetersPerSecond,
configuration.Longitudinal.ZeroSpeedHoldSeconds);
}
/// 最近一次更新产生的不可变执行状态；尚未调用 时为 。
public TrajectoryExecutionState State { get; private set; }
/// 按调用方时刻选择轨迹点并更新换向状态机。
/// 调用方当前时刻；相对轨迹生效时间换算为采样秒数。
/// 车辆测量状态快照；带符号纵向速度单位 m/s，用于判断是否已停稳。
/// 已发布的完整不可变轨迹；开始前取首点、结束后取末点且不外推。
/// 下一方向段要求的实际行驶方向。
/// 调用方确认的当前实际方向。
/// 硬件/调用方是否已确认方向变更完成。
/// 包含选中点、换向状态、停零、换向请求和完成标记的不可变执行状态。
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;
}
/// 更新纯执行状态并返回控制器中立命令。
/// 调用方当前时刻。
/// 车辆测量状态快照。
/// 已发布完整轨迹。
/// 目标实际行驶方向。
/// 已确认当前方向。
/// 是否已确认换向。
/// 常规跟随时输出轨迹 m/s 与 rad/s；停零、等待确认或完成时输出零运动和制动。
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;
}
}