fix: normalize EM PathS resampling residuals

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using System.Globalization;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -9,7 +10,7 @@ internal sealed class TrajectorySampleSchedule
     private const double ZeroTolerance = 1e-12d;
 
     public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
-        EmLongitudinalMode mode, bool resampleMotion)
+        EmLongitudinalMode mode, bool resampleMotion, double pathSTolerance)
     {
         if (candidate == null)
             throw new ArgumentNullException(nameof(candidate));
@@ -17,11 +18,14 @@ internal sealed class TrajectorySampleSchedule
             throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
         if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
             throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
+        if (!IsFinite(pathSTolerance) || pathSTolerance < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathSTolerance));
         if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
             throw new ArgumentOutOfRangeException(nameof(mode));
 
         var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
         double previousPathS = double.NegativeInfinity;
+        double terminalPathS = candidate.S[candidate.S.Count - 1];
         if (resampleMotion)
         {
             double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
@@ -29,9 +33,11 @@ internal sealed class TrajectorySampleSchedule
             for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
                 sampleTime += outputTimeStepSeconds)
             {
-                AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate);
+                AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples.Count, samples,
+                    ref previousPathS, terminalPathS, pathSTolerance, candidate);
             }
-            AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate);
+            AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples.Count, samples,
+                ref previousPathS, terminalPathS, pathSTolerance, candidate);
         }
         else
         {
@@ -39,7 +45,7 @@ internal sealed class TrajectorySampleSchedule
             {
                 AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index],
                     Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d,
-                    false), samples, ref previousPathS, candidate);
+                    false), samples.Count, samples, ref previousPathS, terminalPathS, pathSTolerance, candidate);
             }
         }
 
@@ -130,17 +136,39 @@ internal sealed class TrajectorySampleSchedule
         return false;
     }
 
-    private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
-        ref double previousPathS, LongitudinalCandidate candidate)
+    private static void AddSample(TrajectorySample sample, int sampleIndex, ICollection<TrajectorySample> samples,
+        ref double previousPathS, double terminalPathS, double pathSTolerance, LongitudinalCandidate candidate)
     {
-        if (sample.PathS < previousPathS)
-            throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+        double lowerDifference = previousPathS - sample.PathS;
+        double upperDifference = sample.PathS - terminalPathS;
+        if (lowerDifference > pathSTolerance || upperDifference > pathSTolerance)
+        {
+            throw PathSFailure(sampleIndex, previousPathS, sample.PathS,
+                Math.Max(lowerDifference, upperDifference), candidate);
+        }
+
+        double normalizedPathS = Math.Min(terminalPathS, Math.Max(previousPathS, sample.PathS));
+        if (normalizedPathS != sample.PathS)
+        {
+            sample = new TrajectorySample(sample.TimeFromStart, normalizedPathS, sample.ProgressSpeed,
+                sample.Acceleration, sample.Jerk, sample.IsHoldSample);
+        }
         if (sample.ProgressSpeed < -ZeroTolerance)
             throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
         samples.Add(sample);
         previousPathS = sample.PathS;
     }
 
+    private static ArgumentException PathSFailure(int sampleIndex, double previousPathS,
+        double candidatePathS, double difference, LongitudinalCandidate candidate)
+    {
+        return new ArgumentException("Trajectory PathS violates monotonic publication bounds: sampleIndex=" +
+            sampleIndex.ToString(CultureInfo.InvariantCulture) + ";previousPathS=" +
+            previousPathS.ToString("R", CultureInfo.InvariantCulture) + ";candidatePathS=" +
+            candidatePathS.ToString("R", CultureInfo.InvariantCulture) + ";difference=" +
+            difference.ToString("R", CultureInfo.InvariantCulture) + ".", nameof(candidate));
+    }
+
     private static bool IncrementAndExceedsMaximum(ref int sampleCount, int maximumSampleCount)
     {
         checked { sampleCount++; }