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,131 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace EMPlannerVerificationHost;
+
+internal static class ExecutorChecks
+{
+    public static void Run()
+    {
+        VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse");
+        VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward");
+        VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals();
+    }
+
+    private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
+        string name)
+    {
+        var machine = new GearSwitchStateMachine(0.01d, 0.20d);
+        DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(500d);
+
+        GearSwitchStateUpdate approaching = machine.Update(start, 0.10d, desiredDirection, currentDirection, false, false,
+            false);
+        Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.State, name + " approaches switch");
+        Verification.True(approaching.AllowsTrajectoryMotion, name + " approach permits trajectory motion");
+
+        GearSwitchStateUpdate holding = machine.Update(start.AddMilliseconds(100), 0.005d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(holding, GearSwitchState.HoldingZero, name + " holds at boundary");
+
+        GearSwitchStateUpdate movingAgain = machine.Update(start.AddMilliseconds(110), 0.01d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(movingAgain, GearSwitchState.HoldingZero, name + " holds while measured speed is at tolerance");
+
+        GearSwitchStateUpdate dwellRestart = machine.Update(start.AddMilliseconds(120), 0d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(dwellRestart, GearSwitchState.HoldingZero, name + " starts zero-speed dwell");
+        GearSwitchStateUpdate dwellShort = machine.Update(start.AddMilliseconds(319), 0d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(dwellShort, GearSwitchState.HoldingZero, name + " does not request before full dwell");
+
+        GearSwitchStateUpdate requesting = machine.Update(start.AddMilliseconds(320), 0d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(requesting, GearSwitchState.RequestingDirectionChange, name + " requests after continuous dwell");
+        Verification.True(requesting.RequestDirectionChange, name + " emits exactly one direction request");
+
+        GearSwitchStateUpdate awaiting = machine.Update(start.AddMilliseconds(321), 0d, desiredDirection,
+            currentDirection, false, true, false);
+        AssertHeld(awaiting, GearSwitchState.AwaitingDirectionConfirmation, name + " awaits confirmation");
+        Verification.True(!awaiting.RequestDirectionChange, name + " does not repeat direction request");
+
+        GearSwitchStateUpdate following = machine.Update(start.AddMilliseconds(322), 0d, desiredDirection,
+            desiredDirection, true, false, false);
+        Verification.Equal(GearSwitchState.Following, following.State, name + " follows confirmed next segment");
+        Verification.True(!following.HoldZero && following.AllowsTrajectoryMotion,
+            name + " resumes only after confirmation");
+
+        GearSwitchStateUpdate completed = machine.Update(start.AddMilliseconds(323), 0d, desiredDirection,
+            desiredDirection, false, false, true);
+        AssertHeld(completed, GearSwitchState.Completed, name + " completes at terminal while holding zero");
+        Verification.True(completed.IsTrajectoryComplete, name + " marks terminal completion");
+    }
+
+    private static void VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals()
+    {
+        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(600d);
+        var executor = new TrajectoryExecutor();
+        EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
+            EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
+
+        TrajectoryExecutionState approaching = executor.Update(effectiveAt.AddSeconds(0.10d),
+            CreateMeasuredState(0.10d, effectiveAt.AddSeconds(0.10d), 60L), gearTrajectory, TravelDirection.Reverse,
+            TravelDirection.Forward, false);
+        Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.GearSwitchState,
+            "executor enters approach before exact gear boundary");
+        Verification.NearlyEqual(0.10d, approaching.SelectedPoint.SignedLongitudinalVelocity,
+            "executor samples moving approach point");
+
+        TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
+            CreateMeasuredState(0.005d, effectiveAt.AddSeconds(0.30d), 61L), gearTrajectory, TravelDirection.Reverse,
+            TravelDirection.Forward, false);
+        AssertHeld(holding, GearSwitchState.HoldingZero, "executor holds at exact gear boundary");
+        Verification.Equal(EmBoundaryType.GearSwitchApproach, holding.SelectedPoint.BoundaryType,
+            "executor preserves exact gear boundary point");
+        Verification.NearlyEqual(0d, holding.SelectedPoint.SignedLongitudinalVelocity,
+            "executor does not release nonzero speed while holding");
+
+        foreach (EmBoundaryType terminalBoundary in new[] { EmBoundaryType.Goal, EmBoundaryType.RollingSafetyStop })
+        {
+            var terminalExecutor = new TrajectoryExecutor();
+            EmTrajectory terminal = CreateTrajectory(effectiveAt, TravelDirection.Forward, terminalBoundary,
+                terminalBoundary == EmBoundaryType.Goal ? EmTerminalType.Goal : EmTerminalType.RollingSafetyStop);
+            TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
+                CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 62L), terminal, TravelDirection.Forward,
+                TravelDirection.Forward, false);
+            AssertHeld(completed, GearSwitchState.Completed, "executor completes " + terminalBoundary);
+            Verification.True(completed.IsTrajectoryComplete, "executor marks " + terminalBoundary + " completion");
+        }
+    }
+
+    private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
+    {
+        Verification.Equal(expectedState, update.State, name + " state");
+        Verification.True(update.HoldZero && !update.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
+    }
+
+    private static void AssertHeld(TrajectoryExecutionState state, GearSwitchState expectedState, string name)
+    {
+        Verification.Equal(expectedState, state.GearSwitchState, name + " state");
+        Verification.True(state.HoldZero && !state.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
+    }
+
+    private static VehicleMotionState CreateMeasuredState(double speed, DateTimeOffset capturedAt, long sequenceId)
+    {
+        return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, 0d, capturedAt, sequenceId);
+    }
+
+    private static EmTrajectory CreateTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
+        EmBoundaryType terminalBoundary, EmTerminalType terminalType)
+    {
+        double signedSpeed = direction == TravelDirection.Forward ? 0.10d : -0.10d;
+        var metadata = new EmTrajectoryMetadata("executor-" + terminalBoundary, effectiveAt, effectiveAt, 70L,
+            "executor-reference", 60L, string.Empty, 4, direction, terminalType);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(signedSpeed * 3d, 0d, 0d, 0d, 0.30d, 0d, 4, 0.03d, 0.03d, direction,
+                terminalBoundary, 0d, 0d),
+        });
+    }
+}
@@ -12,7 +12,8 @@ internal static class Program
            args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
            args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
            args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
-            args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator"))
+            args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" &&
+            args[0] != "executor"))
        {
            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
            return 2;
@@ -113,6 +114,11 @@ internal static class Program
                CoordinatorChecks.Run();
                Console.WriteLine("PASS coordinator");
            }
+            if (args[0] == "executor")
+            {
+                ExecutorChecks.Run();
+                Console.WriteLine("PASS executor");
+            }
            return 0;
        }
        catch (Exception exception)