ParkingRobot/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs

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;
    }
}