ParkingRobot/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs

using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Samples immutable EM trajectories and applies the pure zero-speed gear-switch state machine.</summary>
public sealed class TrajectoryExecutor
{
    private readonly TrajectorySampler sampler = new TrajectorySampler();
    private readonly TrajectoryControlAdapter controlAdapter = new TrajectoryControlAdapter();
    private readonly GearSwitchStateMachine gearSwitchStateMachine;

    public TrajectoryExecutor()
        : this(EmPlannerConfiguration.CreateDefault())
    {
    }

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

    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>Updates pure execution state and returns its controller-neutral motion command.</summary>
    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;
    }
}