feat: publish rolling trajectories without stop tails

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)
-            throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
+            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)
     {