feat: publish rolling trajectories without stop tails

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs

@@ -15,7 +15,8 @@ public sealed class EmTrajectoryMetadata
         string previousTrajectoryId,
         int segmentIndex,
         TravelDirection direction,
-        EmTerminalType terminalType)
+        EmTerminalType terminalType,
+        EmLongitudinalMode longitudinalMode)
     {
         if (string.IsNullOrWhiteSpace(trajectoryId))
             throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId));
@@ -31,6 +32,8 @@ public sealed class EmTrajectoryMetadata
             throw new ArgumentOutOfRangeException(nameof(direction));
         if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
             throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), longitudinalMode))
+            throw new ArgumentOutOfRangeException(nameof(longitudinalMode));
 
         TrajectoryId = trajectoryId;
         GeneratedAtUtc = generatedAtUtc;
@@ -42,6 +45,7 @@ public sealed class EmTrajectoryMetadata
         SegmentIndex = segmentIndex;
         Direction = direction;
         TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
     }
 
     public string TrajectoryId { get; }
@@ -54,4 +58,5 @@ public sealed class EmTrajectoryMetadata
     public int SegmentIndex { get; }
     public TravelDirection Direction { get; }
     public EmTerminalType TerminalType { get; }
+    public EmLongitudinalMode LongitudinalMode { get; }
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -97,12 +97,12 @@ public sealed class EmPlanningService : IEmPlanningService
 
             var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                 request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
-                segment.SegmentIndex, segment.Direction, horizon.TerminalType);
+                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode);
             EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
             EmitDebug(request, "trajectory assembly succeeded");
 
             EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
-                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.TerminalPathS,
+                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
                 slice.TerminalBoundary.BoundaryType);
             if (!publication.IsValid)
             {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs

@@ -39,7 +39,8 @@ public sealed class EmTrajectoryAssembler
             throw new ArgumentNullException(nameof(metadata));
 
         var interpolator = new LateralPathInterpolator(path);
-        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds);
+        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds,
+            metadata.LongitudinalMode);
         double terminalPathS = path.Points[path.Points.Count - 1].PathS;
         var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
         double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
@@ -51,7 +52,8 @@ public sealed class EmTrajectoryAssembler
                 throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal));
 
             InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS);
-            bool isTerminalAnchor = index == longitudinal.Candidate.KnotTimes.Count - 1;
+            bool isTerminalAnchor = metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary &&
+                index == schedule.TerminalAnchorSampleIndex;
             EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
             double signedSpeed = directionSign * sample.ProgressSpeed;
             points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

@@ -8,7 +8,8 @@ internal sealed class TrajectorySampleSchedule
 {
     private const double ZeroTolerance = 1e-12d;
 
-    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
+    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
+        EmLongitudinalMode mode)
     {
         if (candidate == null)
             throw new ArgumentNullException(nameof(candidate));
@@ -16,8 +17,8 @@ internal sealed class TrajectorySampleSchedule
             throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
         if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
             throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
-        if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
-            throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
+            throw new ArgumentOutOfRangeException(nameof(mode));
 
         var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
         double previousPathS = double.NegativeInfinity;
@@ -33,6 +34,27 @@ internal sealed class TrajectorySampleSchedule
             previousPathS = candidate.S[index];
         }
 
+        if (mode != EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            Samples = new ReadOnlyCollection<TrajectorySample>(samples);
+            TerminalAnchorSampleIndex = -1;
+            return;
+        }
+
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
+            outputTimeStepSeconds);
+        double stopPathS = candidate.S[stabilizationStart];
+        for (int index = stabilizationStart; index < candidate.S.Count; index++)
+        {
+            if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
+                Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
+                (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
+            {
+                throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
+            }
+        }
+
+        TerminalAnchorSampleIndex = stabilizationStart;
         TrajectorySample terminal = samples[samples.Count - 1];
         double holdElapsed = 0d;
         while (holdElapsed < holdDurationSeconds - ZeroTolerance)
@@ -45,6 +67,7 @@ internal sealed class TrajectorySampleSchedule
     }
 
     public IReadOnlyList<TrajectorySample> Samples { get; }
+    public int TerminalAnchorSampleIndex { get; }
 
     private static bool IsFinite(double value)
     {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs

@@ -16,6 +16,7 @@ public enum EmTrajectoryValidationFailure
     SegmentBoundaryExceeded,
     MissingTerminalAnchor,
     TerminalSpeedNotZero,
+    TerminalAccelerationNotZero,
     TerminalYawRateNotZero,
     DirectionMismatch,
     DirectionSignMismatch,
@@ -77,11 +78,11 @@ public sealed class EmTrajectoryValidator
     }
 
     public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
-        EmPlannerConfiguration configuration, int segmentIndex, double terminalPathS, EmBoundaryType terminalBoundary)
+        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
     {
         if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
             configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
-            !IsFinite(terminalPathS) || terminalPathS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
+            !IsFinite(pathUpperBoundS) || pathUpperBoundS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
             !TryReadLimits(configuration, vehicle, trajectory.Metadata.Direction, out ValidationLimits limits))
         {
             return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
@@ -93,8 +94,8 @@ public sealed class EmTrajectoryValidator
             EmTrajectoryPoint point = trajectory.Points[index];
             if (point == null || !HasOnlyFiniteValues(point))
                 return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
-            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > terminalPathS + limits.SpatialTolerance ||
+            if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
-                point.PathS > terminalPathS + limits.SpatialTolerance)
+                point.PathS > pathUpperBoundS + limits.SpatialTolerance)
             {
                 return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,
                     "Trajectory point lies outside the current direction segment.");
@@ -114,11 +115,14 @@ public sealed class EmTrajectoryValidator
             }
         }
 
-        int terminalIndex = FindTerminalAnchor(trajectory, terminalPathS, terminalBoundary, limits.SpatialTolerance);
+        int terminalIndex = -1;
+        if (trajectory.Metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            terminalIndex = FindTerminalAnchor(trajectory, pathUpperBoundS, terminalBoundary, limits.SpatialTolerance);
             if (terminalIndex < 0)
             {
                 return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
-                FindFirstTerminalPathIndex(trajectory, terminalPathS, limits.SpatialTolerance),
+                    FindFirstTerminalPathIndex(trajectory, pathUpperBoundS, limits.SpatialTolerance),
                     "Trajectory does not contain the exact terminal boundary anchor.");
             }
 
@@ -126,9 +130,28 @@ public sealed class EmTrajectoryValidator
             if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance)
                 return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex,
                     "Terminal signed speed must be zero.");
+            if (Math.Abs(terminal.LongitudinalAcceleration) > limits.KinematicTolerance)
+                return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex,
+                    "Terminal stored acceleration must be zero.");
             if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance)
                 return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
                     "Terminal yaw rate must be zero.");
+            for (int index = terminalIndex; index < trajectory.Points.Count; index++)
+            {
+                EmTrajectoryPoint tail = trajectory.Points[index];
+                if (Math.Abs(tail.PathS - terminal.PathS) > limits.SpatialTolerance)
+                {
+                    return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor, index,
+                        "Exact-stop tail must remain at the terminal PathS.");
+                }
+                if (Math.Abs(tail.SignedLongitudinalVelocity) > limits.KinematicTolerance)
+                    return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, index,
+                        "Exact-stop tail signed speed must remain zero.");
+                if (Math.Abs(tail.LongitudinalAcceleration) > limits.KinematicTolerance)
+                    return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, index,
+                        "Exact-stop tail stored acceleration must remain zero.");
+            }
+        }
 
         double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
         double previousAcceleration = 0d;

ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs

@@ -294,7 +294,9 @@ internal static class CoordinatorChecks
     {
         double signedVelocity = direction == TravelDirection.Forward ? 0.10d : -0.10d;
         var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L,
-            string.Empty, 8, direction, terminalType);
+            string.Empty, 8, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction,
@@ -353,7 +355,8 @@ internal static class CoordinatorChecks
         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);
+            string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d,
@@ -476,7 +479,8 @@ internal static class CoordinatorChecks
         {
             var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                 request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
-                request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop);
+                request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                EmLongitudinalMode.RollingContinuation);
             var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d,
                 TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d);
             return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty);

ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs

@@ -252,7 +252,10 @@ internal static class ExecutorChecks
     {
         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);
+            "executor-reference", 60L, string.Empty, 4, direction, terminalType,
+            terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d),
@@ -265,7 +268,8 @@ internal static class ExecutorChecks
         double signedVelocity, double curvature)
     {
         var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
-            "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal);
+            "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal,
+            EmLongitudinalMode.ExactStopAtBoundary);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -14,6 +14,8 @@ internal static class TrajectoryChecks
         VerifiesForwardFieldsExactTerminalAndHold();
         VerifiesReverseTravelVelocityAndUnwrappedYaw();
         VerifiesPublishedListsAreImmutable();
+        VerifiesRollingTrajectoryHasNoSyntheticStopTail();
+        VerifiesExactStopHoldHasNoJerkDiscontinuity();
         VerifiesWorldSpacePublicationMutationsAreRejected();
         VerifiesReverseSpeedLimitUsesReverseConfiguration();
     }
@@ -50,11 +52,67 @@ internal static class TrajectoryChecks
     {
         EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
             CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
-            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop));
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                EmLongitudinalMode.RollingContinuation));
         Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
             "trajectory public point list is immutable");
     }
 
+    private static void VerifiesRollingTrajectoryHasNoSyntheticStopTail()
+    {
+        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                EmLongitudinalMode.RollingContinuation));
+
+        Verification.Equal(21, trajectory.Points.Count, "rolling trajectory keeps only ST knots");
+        EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
+        Verification.Equal(EmBoundaryType.None, terminal.BoundaryType,
+            "rolling horizon end is not a boundary anchor");
+        Verification.True(terminal.SignedLongitudinalVelocity > 0d,
+            "rolling terminal speed stays nonzero");
+        Verification.Equal(EmLongitudinalMode.RollingContinuation, trajectory.Metadata.LongitudinalMode,
+            "rolling longitudinal mode is published");
+    }
+
+    private static void VerifiesExactStopHoldHasNoJerkDiscontinuity()
+    {
+        ValidationContext context = CreateValidationContext();
+        EmTrajectory trajectory = CreateValidationTrajectory(TravelDirection.Forward);
+        EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, context.EmptyMap,
+            context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
+        Verification.True(result.IsValid, "exact stop with internal tail and external hold is publishable: " + result.Message);
+
+        int anchorIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
+        EmTrajectoryPoint previousMoving = trajectory.Points[anchorIndex - 1];
+        EmTrajectoryPoint anchor = trajectory.Points[anchorIndex];
+        EmTrajectoryPoint stabilization = trajectory.Points[anchorIndex + 1];
+        EmTrajectoryPoint firstExternalHold = trajectory.Points[anchorIndex + 2];
+        Verification.Equal(EmBoundaryType.Goal, anchor.BoundaryType, "exact stop marks only the real boundary anchor");
+        Verification.Equal(EmBoundaryType.None, stabilization.BoundaryType,
+            "QP stabilization point is not a duplicate boundary anchor");
+        Verification.Equal(EmBoundaryType.None, firstExternalHold.BoundaryType,
+            "external hold is not a duplicate boundary anchor");
+
+        double terminalFiniteDifferenceAcceleration =
+            (anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
+            (anchor.TimeFromStart - previousMoving.TimeFromStart);
+        double stabilizationAcceleration =
+            (stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
+            (stabilization.TimeFromStart - anchor.TimeFromStart);
+        double externalHoldAcceleration =
+            (firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
+            (firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
+        double jerkIntoStabilization = (stabilizationAcceleration - terminalFiniteDifferenceAcceleration) /
+            (stabilization.TimeFromStart - anchor.TimeFromStart);
+        double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
+            (firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
+        Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
+            "jerk into QP stabilization stays within the limit");
+        Verification.NearlyEqual(0d, jerkIntoExternalHold, "jerk into the external hold is zero");
+    }
+
     private static void VerifiesWorldSpacePublicationMutationsAreRejected()
     {
         ValidationContext context = CreateValidationContext();
@@ -104,11 +162,14 @@ internal static class TrajectoryChecks
         AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 0.75d)),
             EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, 1, "curvature-rate limit");
 
-        const int terminalIndex = 8;
+        int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
         AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], boundaryType: EmBoundaryType.None)),
-            EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex, "missing exact terminal anchor");
+            EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex - 1, "missing exact terminal anchor");
         AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], signedSpeed: 0.01d)),
             EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex, "terminal speed");
+        AssertRejected(context, CorruptDouble(valid, terminalIndex, "LongitudinalAcceleration", 0.01d),
+            EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex, "terminal acceleration");
         AssertRejected(context, CorruptDouble(valid, terminalIndex, "YawRate", 0.01d),
             EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex, "terminal yaw rate");
 
@@ -186,13 +247,7 @@ internal static class TrajectoryChecks
 
     private static EmTrajectory CreateValidationTrajectory(TravelDirection direction)
     {
-        double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d };
+        LongitudinalCandidate candidate = CreateValidationCandidate();
-        double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
-            0.00546875d, 0.0055d };
-        double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d };
-        double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d };
-        double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d };
-        var candidate = new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
         var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
         var path = new LateralPath(new[]
         {
@@ -202,6 +257,17 @@ internal static class TrajectoryChecks
         return new EmTrajectoryAssembler().Assemble(path, result, CreateMetadata(direction, EmTerminalType.Goal));
     }
 
+    private static LongitudinalCandidate CreateValidationCandidate()
+    {
+        double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d, 0.45d, 0.50d };
+        double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
+            0.00546875d, 0.0055d, 0.0055d, 0.0055d };
+        double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d, 0d, 0d };
+        double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d, 0d, 0d };
+        double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0d, 0d };
+        return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+    }
+
     private static EmTrajectory Replace(EmTrajectory trajectory, int index, EmTrajectoryPoint replacement)
     {
         var points = new List<EmTrajectoryPoint>(trajectory.Points);
@@ -280,8 +346,8 @@ internal static class TrajectoryChecks
         Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type");
         Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed");
         Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate");
-        Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time");
+        Verification.NearlyEqual(0.15d, terminal.TimeFromStart, name + " exact terminal stabilization time");
-        Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
+        Verification.Equal(terminalIndex + 6, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
 
         for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++)
         {
@@ -308,17 +374,35 @@ internal static class TrajectoryChecks
     private static LongitudinalPlanningResult CreateLongitudinalResult()
     {
         var candidate = new LongitudinalCandidate(
-            new[] { 0d, 0.05d, 0.10d, 0.137d },
+            new[] { 0d, 0.05d, 0.10d, 0.15d, 0.20d },
-            new[] { 0d, 0.04d, 0.08d, 0.12d },
+            new[] { 0d, 0.04d, 0.08d, 0.12d, 0.12d },
-            new[] { 0.8d, 0.8d, 0.8d, 0d },
+            new[] { 0.8d, 0.8d, 0.2d, 0d, 0d },
-            new[] { 0d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d, 0d, 0d },
-            new[] { 0d, 0d, 0d });
+            new[] { 0d, 0d, 0d, 0d });
         return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
     }
 
-    private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType)
+    private static LongitudinalPlanningResult CreateRollingLongitudinalResult()
+    {
+        var times = new double[21];
+        var pathS = new double[21];
+        var speed = new double[21];
+        var acceleration = new double[21];
+        var jerk = new double[20];
+        for (int index = 0; index < times.Length; index++)
+        {
+            times[index] = index * 0.1d;
+            pathS[index] = index * 0.004d;
+            speed[index] = 0.04d;
+        }
+        return new LongitudinalPlanningResult(EmPlanningStatus.Success,
+            new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
+    }
+
+    private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
+        EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
     {
         return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
-            "reference", 4L, string.Empty, 2, direction, terminalType);
+            "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode);
     }
 }

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -771,7 +771,8 @@ internal static class TrajectoryObservationChecks
         EmTerminalType terminalType = EmTerminalType.Goal)
     {
         var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
-            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType);
+            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType,
+            EmLongitudinalMode.RollingContinuation);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,