feat: select safe EM trajectory handoffs

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Threading;
 using System.Threading.Tasks;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -12,6 +13,7 @@ public sealed class EmPlanningCoordinator
 {
     private readonly IEmPlanningService planningService;
     private readonly IEmPlanningCycleSink sink;
+    private readonly TrajectoryHandoffSelector handoffSelector = new TrajectoryHandoffSelector();
     private readonly object publicationGate = new object();
     private long latestCycleVersion;
     private PlanningCycleIdentity latestIdentity;
@@ -66,6 +68,19 @@ public sealed class EmPlanningCoordinator
         return Task.Run(() => CompleteCycle(version, input, cycleCancellation));
     }
 
+    /// <summary>Builds a handoff decision from the coordinator's immutable published trajectory snapshot.</summary>
+    public TrajectoryHandoffSelection SelectHandoff(PlanningCycleInput input, TravelDirection expectedDirection)
+    {
+        if (input == null)
+            throw new ArgumentNullException(nameof(input));
+
+        EmTrajectory trajectory;
+        lock (publicationGate)
+            trajectory = publishedTrajectory;
+        return handoffSelector.Select(trajectory, input.Request.VehicleState, input.Identity.SegmentIndex, expectedDirection,
+            input.Now, input.Request.Configuration);
+    }
+
     private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input,
         CancellationTokenSource cycleCancellation)
     {

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs

@@ -0,0 +1,171 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum TrajectoryHandoffSource
+{
+    MeasuredState,
+    PreviousTrajectory,
+}
+
+public enum TrajectoryHandoffRejectionReason
+{
+    None,
+    MissingTrajectory,
+    InvalidConfiguration,
+    TrajectoryNotYetEffective,
+    TrajectoryTooOld,
+    SegmentMismatch,
+    DirectionMismatch,
+    TrackingErrorExceeded,
+    GearBoundary,
+    TerminalBoundary,
+    HandoffBeyondTrajectory,
+    SampleUnavailable,
+}
+
+/// <summary>Immutable handoff decision for the next one-shot planning request.</summary>
+public sealed class TrajectoryHandoffSelection
+{
+    internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState,
+        EmTrajectory previousTrajectory, EmTrajectoryPoint sampledPoint, TrajectoryHandoffRejectionReason rejectionReason)
+    {
+        Source = source;
+        StartState = startState ?? throw new ArgumentNullException(nameof(startState));
+        PreviousTrajectory = previousTrajectory;
+        SampledPoint = sampledPoint;
+        RejectionReason = rejectionReason;
+    }
+
+    public TrajectoryHandoffSource Source { get; }
+
+    public VehicleMotionState StartState { get; }
+
+    public EmTrajectory PreviousTrajectory { get; }
+
+    public EmTrajectoryPoint SampledPoint { get; }
+
+    public TrajectoryHandoffRejectionReason RejectionReason { get; }
+}
+
+/// <summary>Selects a future same-segment seed or preserves the caller's measured state.</summary>
+public sealed class TrajectoryHandoffSelector
+{
+    private const double TimeEpsilonSeconds = 1e-9d;
+    private readonly TrajectorySampler sampler;
+
+    public TrajectoryHandoffSelector()
+        : this(new TrajectorySampler())
+    {
+    }
+
+    internal TrajectoryHandoffSelector(TrajectorySampler sampler)
+    {
+        this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler));
+    }
+
+    public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState,
+        int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now,
+        EmPlannerConfiguration configuration)
+    {
+        if (measuredState == null)
+            throw new ArgumentNullException(nameof(measuredState));
+        if (expectedSegmentIndex < 0 || !Enum.IsDefined(typeof(TravelDirection), expectedDirection))
+            throw new ArgumentOutOfRangeException(nameof(expectedSegmentIndex));
+        if (previousTrajectory == null)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.MissingTrajectory);
+        if (!TryReadConfiguration(configuration, out double maximumAgeSeconds, out double lookaheadSeconds,
+                out double spatialToleranceMeters, out double kinematicTolerance))
+        {
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.InvalidConfiguration);
+        }
+        if (previousTrajectory.Metadata.SegmentIndex != expectedSegmentIndex)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.SegmentMismatch);
+        if (previousTrajectory.Metadata.Direction != expectedDirection)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.DirectionMismatch);
+
+        double trajectoryAgeSeconds = (now - previousTrajectory.Metadata.EffectiveAtUtc).TotalSeconds;
+        if (trajectoryAgeSeconds < -TimeEpsilonSeconds)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryNotYetEffective);
+        if (trajectoryAgeSeconds > maximumAgeSeconds + TimeEpsilonSeconds)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryTooOld);
+        if (!sampler.TrySample(previousTrajectory, trajectoryAgeSeconds, out EmTrajectoryPoint currentPoint))
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
+        if (!Tracks(currentPoint, measuredState, spatialToleranceMeters, kinematicTolerance))
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrackingErrorExceeded);
+
+        double handoffTime = trajectoryAgeSeconds + lookaheadSeconds;
+        double terminalTime = previousTrajectory.Points[previousTrajectory.Points.Count - 1].TimeFromStart;
+        if (handoffTime > terminalTime + TimeEpsilonSeconds)
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory);
+        TrajectoryHandoffRejectionReason boundaryReason = BoundaryInInterval(previousTrajectory, trajectoryAgeSeconds,
+            handoffTime);
+        if (boundaryReason != TrajectoryHandoffRejectionReason.None)
+            return Measured(measuredState, boundaryReason);
+        if (!sampler.TrySample(previousTrajectory, handoffTime, out EmTrajectoryPoint handoffPoint))
+            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
+
+        var startState = new VehicleMotionState(new Pose2D(handoffPoint.X, handoffPoint.Y, handoffPoint.Yaw),
+            handoffPoint.SignedLongitudinalVelocity, handoffPoint.LongitudinalAcceleration,
+            previousTrajectory.Metadata.EffectiveAtUtc.AddSeconds(handoffTime), measuredState.SequenceId);
+        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.PreviousTrajectory, startState,
+            previousTrajectory, handoffPoint, TrajectoryHandoffRejectionReason.None);
+    }
+
+    private static TrajectoryHandoffSelection Measured(VehicleMotionState measuredState,
+        TrajectoryHandoffRejectionReason rejectionReason)
+    {
+        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.MeasuredState, measuredState, null, null,
+            rejectionReason);
+    }
+
+    private static bool TryReadConfiguration(EmPlannerConfiguration configuration, out double maximumAgeSeconds,
+        out double lookaheadSeconds, out double spatialToleranceMeters, out double kinematicTolerance)
+    {
+        maximumAgeSeconds = configuration?.Scheduling?.MaximumVehicleStateAgeSeconds ?? double.NaN;
+        lookaheadSeconds = configuration?.Scheduling?.HandoffLookaheadSeconds ?? double.NaN;
+        spatialToleranceMeters = configuration?.Validation?.SpatialToleranceMeters ?? double.NaN;
+        kinematicTolerance = configuration?.Validation?.KinematicTolerance ?? double.NaN;
+        return IsNonNegativeFinite(maximumAgeSeconds) && IsNonNegativeFinite(lookaheadSeconds) &&
+            IsNonNegativeFinite(spatialToleranceMeters) && IsNonNegativeFinite(kinematicTolerance);
+    }
+
+    private static bool Tracks(EmTrajectoryPoint trajectoryPoint, VehicleMotionState measuredState,
+        double spatialToleranceMeters, double kinematicTolerance)
+    {
+        double dx = trajectoryPoint.X - measuredState.Pose.X;
+        double dy = trajectoryPoint.Y - measuredState.Pose.Y;
+        if (Math.Sqrt(dx * dx + dy * dy) > spatialToleranceMeters)
+            return false;
+        double yawError = Math.Atan2(Math.Sin(trajectoryPoint.Yaw - measuredState.Pose.Heading),
+            Math.Cos(trajectoryPoint.Yaw - measuredState.Pose.Heading));
+        return Math.Abs(yawError) <= kinematicTolerance &&
+            Math.Abs(trajectoryPoint.SignedLongitudinalVelocity -
+                measuredState.SignedLongitudinalSpeedMetersPerSecond) <= kinematicTolerance;
+    }
+
+    private static TrajectoryHandoffRejectionReason BoundaryInInterval(EmTrajectory trajectory, double startTime,
+        double endTime)
+    {
+        for (int index = 0; index < trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = trajectory.Points[index];
+            if (point.TimeFromStart <= startTime + TimeEpsilonSeconds || point.TimeFromStart > endTime + TimeEpsilonSeconds)
+                continue;
+            if (point.BoundaryType == EmBoundaryType.GearSwitchApproach ||
+                point.BoundaryType == EmBoundaryType.GearSwitchDeparture)
+            {
+                return TrajectoryHandoffRejectionReason.GearBoundary;
+            }
+            if (point.BoundaryType == EmBoundaryType.RollingSafetyStop || point.BoundaryType == EmBoundaryType.Goal)
+                return TrajectoryHandoffRejectionReason.TerminalBoundary;
+        }
+        return TrajectoryHandoffRejectionReason.None;
+    }
+
+    private static bool IsNonNegativeFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs

@@ -0,0 +1,85 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Interpolates an immutable trajectory only inside one homogeneous trajectory interval.</summary>
+public sealed class TrajectorySampler
+{
+    private const double TimeEpsilonSeconds = 1e-9d;
+
+    public bool TrySample(EmTrajectory trajectory, double timeFromStart, out EmTrajectoryPoint sampledPoint)
+    {
+        sampledPoint = null;
+        if (trajectory == null || double.IsNaN(timeFromStart) || double.IsInfinity(timeFromStart) ||
+            trajectory.Points.Count == 0)
+        {
+            return false;
+        }
+
+        EmTrajectoryPoint first = trajectory.Points[0];
+        EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1];
+        if (timeFromStart < first.TimeFromStart - TimeEpsilonSeconds ||
+            timeFromStart > last.TimeFromStart + TimeEpsilonSeconds)
+        {
+            return false;
+        }
+
+        int lower = 0;
+        int upper = trajectory.Points.Count - 1;
+        while (upper - lower > 1)
+        {
+            int middle = lower + (upper - lower) / 2;
+            EmTrajectoryPoint point = trajectory.Points[middle];
+            if (Math.Abs(point.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
+            {
+                sampledPoint = point;
+                return true;
+            }
+
+            if (point.TimeFromStart < timeFromStart)
+                lower = middle;
+            else
+                upper = middle;
+        }
+
+        EmTrajectoryPoint left = trajectory.Points[lower];
+        EmTrajectoryPoint right = trajectory.Points[upper];
+        if (Math.Abs(left.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
+        {
+            sampledPoint = left;
+            return true;
+        }
+        if (Math.Abs(right.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds)
+        {
+            sampledPoint = right;
+            return true;
+        }
+        if (left.SegmentIndex != right.SegmentIndex || left.Direction != right.Direction ||
+            left.BoundaryType != right.BoundaryType)
+        {
+            return false;
+        }
+
+        double fraction = (timeFromStart - left.TimeFromStart) / (right.TimeFromStart - left.TimeFromStart);
+        sampledPoint = new EmTrajectoryPoint(
+            Interpolate(left.X, right.X, fraction),
+            Interpolate(left.Y, right.Y, fraction),
+            Interpolate(left.Yaw, right.Yaw, fraction),
+            Interpolate(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, fraction),
+            timeFromStart,
+            Interpolate(left.VehicleCurvature, right.VehicleCurvature, fraction),
+            left.SegmentIndex,
+            Interpolate(left.SegmentLocalS, right.SegmentLocalS, fraction),
+            Interpolate(left.PathS, right.PathS, fraction),
+            left.Direction,
+            left.BoundaryType,
+            Interpolate(left.LongitudinalAcceleration, right.LongitudinalAcceleration, fraction),
+            Interpolate(left.LongitudinalJerk, right.LongitudinalJerk, fraction));
+        return true;
+    }
+
+    private static double Interpolate(double left, double right, double fraction)
+    {
+        return left + (right - left) * fraction;
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs

@@ -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();
+        configuration ??= EmPlannerConfiguration.CreateDefault();
-        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
+        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)