fix: normalize EM PathS resampling residuals

2026-08-10 14:24:15 +08:00
parent 8a1ba2a6d5
commit 06d9037645
2 changed files with 43 additions and 11 deletions
@@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler
 {
    private readonly double outputTimeStepSeconds;
    private readonly double zeroSpeedHoldSeconds;
+    private readonly double pathSTolerance;
    private readonly int maximumPublishedSampleCount;

    public EmTrajectoryAssembler()
@@ -18,13 +19,16 @@ public sealed class EmTrajectoryAssembler

    public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
    {
-        if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
+        if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
+            configuration.Validation == null)
            throw new ArgumentNullException(nameof(configuration));
        outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
        zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
+        pathSTolerance = configuration.Validation.KinematicTolerance;
        maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
-            zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
+            zeroSpeedHoldSeconds < 0d || !IsFinite(pathSTolerance) || pathSTolerance < 0d ||
+            maximumPublishedSampleCount < 2)
        {
            throw new ArgumentOutOfRangeException(nameof(configuration));
        }
@@ -64,7 +68,7 @@ public sealed class EmTrajectoryAssembler
            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
        }
        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
-            metadata.LongitudinalMode, isFullDirectionSegment);
+            metadata.LongitudinalMode, isFullDirectionSegment, pathSTolerance);
        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;
@@ -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++; }