feat: execute EM gear-switch boundaries

2026-08-04 13:01:59 +08:00
parent 55119de1c7
commit de402e61ee
6 changed files with 394 additions and 1 deletions
@@ -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;
+    }
+}