feat: select safe EM trajectory handoffs

2026-08-04 12:54:53 +08:00
parent d75380cf8f
commit 55119de1c7
4 changed files with 409 additions and 5 deletions
@@ -16,6 +16,7 @@ internal static class CoordinatorChecks
        VerifiesCompletedCycleDoesNotPoisonNextCancellationSource();
        VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
        VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics();
+        VerifiesSafePreviousTrajectoryHandoffs();
    }

    private static void VerifiesCallerSuppliedSchedulingDecision()
@@ -96,6 +97,134 @@ internal static class CoordinatorChecks
            "sink failure is reported in diagnostic");
    }

+    private static void VerifiesSafePreviousTrajectoryHandoffs()
+    {
+        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(300d);
+        EmPlannerConfiguration configuration = CreateHandoffConfiguration();
+        var selector = new TrajectoryHandoffSelector();
+        EmTrajectory forward = CreateHandoffTrajectory("handoff-forward", TravelDirection.Forward, 2, effectiveAt,
+            EmBoundaryType.RollingSafetyStop, false);
+        VehicleMotionState forwardMeasured = CreateMeasuredState(0.01d, 0d, 3.1666666666666665d, 0.10d,
+            effectiveAt.AddSeconds(0.10d), 40L);
+
+        TrajectoryHandoffSelection accepted = selector.Select(forward, forwardMeasured, 2, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.10d), configuration);
+        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, accepted.Source, "forward handoff source");
+        Verification.Equal(TrajectoryHandoffRejectionReason.None, accepted.RejectionReason, "forward handoff reason");
+        Verification.True(object.ReferenceEquals(forward, accepted.PreviousTrajectory), "forward handoff seed");
+        Verification.NearlyEqual(0.04d, accepted.StartState.Pose.X, "forward handoff position interpolation");
+        Verification.NearlyEqual(3.3666666666666667d, accepted.StartState.Pose.Heading,
+            "forward handoff yaw remains unwrapped");
+        Verification.NearlyEqual(0.10d, accepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
+            "forward handoff signed speed interpolation");
+        Verification.Equal(40L, accepted.StartState.SequenceId, "handoff retains measured-state identity");
+
+        EmTrajectory reverse = CreateHandoffTrajectory("handoff-reverse", TravelDirection.Reverse, 2, effectiveAt,
+            EmBoundaryType.RollingSafetyStop, false);
+        VehicleMotionState reverseMeasured = CreateMeasuredState(-0.01d, 0d, 3.1666666666666665d, -0.10d,
+            effectiveAt.AddSeconds(0.10d), 41L);
+        TrajectoryHandoffSelection reverseAccepted = selector.Select(reverse, reverseMeasured, 2, TravelDirection.Reverse,
+            effectiveAt.AddSeconds(0.10d), configuration);
+        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, reverseAccepted.Source,
+            "reverse same-segment handoff source");
+        Verification.NearlyEqual(-0.04d, reverseAccepted.StartState.Pose.X, "reverse handoff position interpolation");
+        Verification.NearlyEqual(-0.10d, reverseAccepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
+            "reverse handoff signed speed interpolation");
+
+        AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.21d), configuration, TrajectoryHandoffRejectionReason.TrajectoryTooOld,
+            "stale trajectory handoff");
+        configuration.Scheduling.MaximumVehicleStateAgeSeconds = 0.60d;
+        AssertHandoffRejected(selector, forward,
+            CreateMeasuredState(1d, 0d, 3.1666666666666665d, 0.10d, effectiveAt.AddSeconds(0.10d), 42L), 2,
+            TravelDirection.Forward, effectiveAt.AddSeconds(0.10d), configuration,
+            TrajectoryHandoffRejectionReason.TrackingErrorExceeded, "large tracking-error handoff");
+        VehicleMotionState terminalMeasured = CreateMeasuredState(0.03d, 0d, 3.30d, 0.10d,
+            effectiveAt.AddSeconds(0.30d), 43L);
+        AssertHandoffRejected(selector, forward, terminalMeasured, 2, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.30d), configuration, TrajectoryHandoffRejectionReason.TerminalBoundary,
+            "terminal-proximity handoff");
+        AssertHandoffRejected(selector, forward, forwardMeasured, 3, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.SegmentMismatch,
+            "segment-mismatch handoff");
+        AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Reverse,
+            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.DirectionMismatch,
+            "direction-mismatch handoff");
+        VehicleMotionState beyondMeasured = CreateMeasuredState(0.031d, 0d, 3.3066666666666666d, 0.10d,
+            effectiveAt.AddSeconds(0.31d), 44L);
+        AssertHandoffRejected(selector, forward, beyondMeasured, 2, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.31d), configuration, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory,
+            "beyond-trajectory handoff");
+
+        EmTrajectory gearBoundary = CreateHandoffTrajectory("handoff-gear", TravelDirection.Forward, 2, effectiveAt,
+            EmBoundaryType.RollingSafetyStop, true);
+        AssertHandoffRejected(selector, gearBoundary, forwardMeasured, 2, TravelDirection.Forward,
+            effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.GearBoundary,
+            "gear-boundary handoff");
+        Verification.True(!new TrajectorySampler().TrySample(gearBoundary, 0.15d, out _),
+            "sampler never interpolates across different boundary types");
+
+        var service = new ControlledPlanningService();
+        var coordinator = new EmPlanningCoordinator(service);
+        PlanningCycleInput coordinatorInput = CreateInput(CreateMap(22), "handoff-reference", 40L, "handoff-forward",
+            2, "handoff-publish", effectiveAt.AddSeconds(0.10d), configuration, forwardMeasured);
+        Task<PlanningCycleResult> publication = coordinator.PlanLatestAsync(coordinatorInput, CancellationToken.None);
+        service.WaitUntilStarted(coordinatorInput.Request.OutputTrajectoryId);
+        service.Complete(coordinatorInput.Request.OutputTrajectoryId, forward);
+        publication.GetAwaiter().GetResult();
+        TrajectoryHandoffSelection coordinatorSelection = coordinator.SelectHandoff(coordinatorInput, TravelDirection.Forward);
+        Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, coordinatorSelection.Source,
+            "coordinator consumes only its published immutable trajectory");
+    }
+
+    private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory,
+        VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now,
+        EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name)
+    {
+        TrajectoryHandoffSelection selection = selector.Select(trajectory, measuredState, segmentIndex, direction, now,
+            configuration);
+        Verification.Equal(TrajectoryHandoffSource.MeasuredState, selection.Source, name + " source");
+        Verification.Equal(reason, selection.RejectionReason, name + " reason");
+        Verification.True(selection.PreviousTrajectory == null, name + " has no seed");
+        Verification.True(object.ReferenceEquals(measuredState, selection.StartState), name + " returns measured state");
+    }
+
+    private static EmPlannerConfiguration CreateHandoffConfiguration()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Validation.SpatialToleranceMeters = 0.05d;
+        configuration.Validation.KinematicTolerance = 0.05d;
+        return configuration;
+    }
+
+    private static VehicleMotionState CreateMeasuredState(double x, double y, double yaw, double signedSpeed,
+        DateTimeOffset capturedAt, long sequenceId)
+    {
+        return new VehicleMotionState(new Pose2D(x, y, yaw), signedSpeed, 0d, capturedAt, sequenceId);
+    }
+
+    private static EmTrajectory CreateHandoffTrajectory(string trajectoryId, TravelDirection direction, int segmentIndex,
+        DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, bool includeGearBoundary)
+    {
+        double sign = direction == TravelDirection.Forward ? 1d : -1d;
+        EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None;
+        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L,
+            string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.01d, 0d, 3.1666666666666665d, sign * 0.10d, 0.10d, 0.25d,
+                segmentIndex, 0.01d, 0.01d, direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.03d, 0d, 3.30d, sign * 0.10d, 0.30d, 0.25d, segmentIndex, 0.03d,
+                0.03d, direction, middleBoundary, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.055d, 0d, 3.4666666666666668d, sign * 0.10d, 0.55d, 0.25d,
+                segmentIndex, 0.055d, 0.055d, direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.06d, 0d, 3.50d, 0d, 0.60d, 0.25d, segmentIndex, 0.06d, 0.06d,
+                direction, terminalBoundary, 0d, 0d),
+        });
+    }
+
    private static void VerifySuperseded(string name, PlanningCycleInput older, PlanningCycleInput newer)
    {
        var service = new ControlledPlanningService();
@@ -121,10 +250,11 @@ internal static class CoordinatorChecks
    }

    private static PlanningCycleInput CreateInput(PlanningGridMap map, string referencePathId, long stateSequenceId,
-        string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now)
+        string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now,
+        EmPlannerConfiguration? configuration = null, VehicleMotionState? state = null)
    {
-        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
-        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
+        configuration ??= EmPlannerConfiguration.CreateDefault();
+        state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
        var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now,
            outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse);
        return new PlanningCycleInput(request, now);
@@ -179,12 +309,15 @@ internal static class CoordinatorChecks
                return pending[outputTrajectoryId].CancellationToken.IsCancellationRequested;
        }

-        public void Complete(string outputTrajectoryId)
+        public void Complete(string outputTrajectoryId, EmTrajectory? trajectory = null)
        {
            PendingCycle cycle;
            lock (gate)
                cycle = pending[outputTrajectoryId];
-            cycle.Completion.TrySetResult(CreateSuccess(cycle.Request));
+            EmPlanningResult result = trajectory == null
+                ? CreateSuccess(cycle.Request)
+                : new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
+            cycle.Completion.TrySetResult(result);
        }

        private static EmPlanningResult CreateSuccess(EmPlanningRequest request)