feat: execute EM gear-switch boundaries

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs

@@ -0,0 +1,11 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum GearSwitchState
+{
+    Following,
+    ApproachingGearSwitch,
+    HoldingZero,
+    RequestingDirectionChange,
+    AwaitingDirectionConfirmation,
+    Completed,
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs

@@ -0,0 +1,144 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Pure caller-clocked state machine for a zero-speed direction change.</summary>
+public sealed class GearSwitchStateMachine
+{
+    private readonly double stopSpeedToleranceMetersPerSecond;
+    private readonly TimeSpan zeroSpeedHoldDuration;
+    private DateTimeOffset? zeroSpeedSince;
+
+    public GearSwitchStateMachine(double stopSpeedToleranceMetersPerSecond, double zeroSpeedHoldSeconds)
+    {
+        if (!IsNonNegativeFinite(stopSpeedToleranceMetersPerSecond))
+            throw new ArgumentOutOfRangeException(nameof(stopSpeedToleranceMetersPerSecond));
+        if (!IsNonNegativeFinite(zeroSpeedHoldSeconds))
+            throw new ArgumentOutOfRangeException(nameof(zeroSpeedHoldSeconds));
+
+        this.stopSpeedToleranceMetersPerSecond = stopSpeedToleranceMetersPerSecond;
+        zeroSpeedHoldDuration = TimeSpan.FromSeconds(zeroSpeedHoldSeconds);
+    }
+
+    public GearSwitchState State { get; private set; } = GearSwitchState.Following;
+
+    public GearSwitchStateUpdate Update(DateTimeOffset now, double measuredSignedSpeed, TravelDirection desiredDirection,
+        TravelDirection currentDirection, bool directionConfirmed, bool atGearSwitchBoundary, bool atTerminal)
+    {
+        if (double.IsNaN(measuredSignedSpeed) || double.IsInfinity(measuredSignedSpeed))
+            throw new ArgumentOutOfRangeException(nameof(measuredSignedSpeed));
+        if (!Enum.IsDefined(typeof(TravelDirection), desiredDirection) ||
+            !Enum.IsDefined(typeof(TravelDirection), currentDirection))
+        {
+            throw new ArgumentOutOfRangeException(nameof(desiredDirection));
+        }
+
+        if (atTerminal)
+        {
+            State = GearSwitchState.Completed;
+            zeroSpeedSince = null;
+            return Held("terminal boundary reached", false, true);
+        }
+
+        switch (State)
+        {
+            case GearSwitchState.Following:
+                if (desiredDirection != currentDirection)
+                {
+                    State = GearSwitchState.ApproachingGearSwitch;
+                    return Moving("approaching gear-switch boundary");
+                }
+                return Moving("following current direction segment");
+
+            case GearSwitchState.ApproachingGearSwitch:
+                if (!atGearSwitchBoundary)
+                    return Moving("approaching gear-switch boundary");
+                State = GearSwitchState.HoldingZero;
+                zeroSpeedSince = IsStopped(measuredSignedSpeed) ? now : null;
+                return Held("holding at gear-switch boundary", false, false);
+
+            case GearSwitchState.HoldingZero:
+                if (!IsStopped(measuredSignedSpeed))
+                {
+                    zeroSpeedSince = null;
+                    return Held("measured speed remains above stop tolerance", false, false);
+                }
+                if (!zeroSpeedSince.HasValue)
+                {
+                    zeroSpeedSince = now;
+                    return Held("zero-speed dwell started", false, false);
+                }
+                if (now - zeroSpeedSince.Value < zeroSpeedHoldDuration)
+                    return Held("zero-speed dwell in progress", false, false);
+
+                State = GearSwitchState.RequestingDirectionChange;
+                return Held("requesting direction change", true, false);
+
+            case GearSwitchState.RequestingDirectionChange:
+                State = GearSwitchState.AwaitingDirectionConfirmation;
+                return Held("awaiting direction confirmation", false, false);
+
+            case GearSwitchState.AwaitingDirectionConfirmation:
+                if (directionConfirmed && desiredDirection == currentDirection)
+                {
+                    State = GearSwitchState.Following;
+                    zeroSpeedSince = null;
+                    return Moving("direction change confirmed");
+                }
+                return Held("awaiting direction confirmation", false, false);
+
+            case GearSwitchState.Completed:
+                return Held("trajectory already completed", false, true);
+
+            default:
+                throw new InvalidOperationException("Unsupported gear-switch state.");
+        }
+    }
+
+    private GearSwitchStateUpdate Moving(string reason)
+    {
+        return new GearSwitchStateUpdate(State, false, false, true, false, reason);
+    }
+
+    private GearSwitchStateUpdate Held(string reason, bool requestDirectionChange, bool isTrajectoryComplete)
+    {
+        return new GearSwitchStateUpdate(State, true, requestDirectionChange, false, isTrajectoryComplete, reason);
+    }
+
+    private bool IsStopped(double measuredSignedSpeed)
+    {
+        return Math.Abs(measuredSignedSpeed) < stopSpeedToleranceMetersPerSecond;
+    }
+
+    private static bool IsNonNegativeFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
+    }
+}
+
+public sealed class GearSwitchStateUpdate
+{
+    internal GearSwitchStateUpdate(GearSwitchState state, bool holdZero, bool requestDirectionChange,
+        bool allowsTrajectoryMotion, bool isTrajectoryComplete, string reason)
+    {
+        State = state;
+        HoldZero = holdZero;
+        RequestDirectionChange = requestDirectionChange;
+        AllowsTrajectoryMotion = allowsTrajectoryMotion;
+        IsTrajectoryComplete = isTrajectoryComplete;
+        Reason = reason ?? string.Empty;
+    }
+
+    public GearSwitchState State { get; }
+
+    public bool HoldZero { get; }
+
+    public bool RequestDirectionChange { get; }
+
+    public bool AllowsTrajectoryMotion { get; }
+
+    public bool IsTrajectoryComplete { get; }
+
+    public string Reason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs

@@ -0,0 +1,35 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable trajectory-execution outcome without controller or hardware coupling.</summary>
+public sealed class TrajectoryExecutionState
+{
+    internal TrajectoryExecutionState(EmTrajectoryPoint selectedPoint, GearSwitchStateUpdate gearSwitchUpdate)
+    {
+        SelectedPoint = selectedPoint ?? throw new ArgumentNullException(nameof(selectedPoint));
+        if (gearSwitchUpdate == null)
+            throw new ArgumentNullException(nameof(gearSwitchUpdate));
+
+        GearSwitchState = gearSwitchUpdate.State;
+        HoldZero = gearSwitchUpdate.HoldZero;
+        RequestDirectionChange = gearSwitchUpdate.RequestDirectionChange;
+        AllowsTrajectoryMotion = gearSwitchUpdate.AllowsTrajectoryMotion;
+        IsTrajectoryComplete = gearSwitchUpdate.IsTrajectoryComplete;
+        Reason = gearSwitchUpdate.Reason;
+    }
+
+    public EmTrajectoryPoint SelectedPoint { get; }
+
+    public GearSwitchState GearSwitchState { get; }
+
+    public bool HoldZero { get; }
+
+    public bool RequestDirectionChange { get; }
+
+    public bool AllowsTrajectoryMotion { get; }
+
+    public bool IsTrajectoryComplete { get; }
+
+    public string Reason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs

@@ -0,0 +1,66 @@
+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 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;
+    }
+
+    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;
+    }
+}