feat: validate EM terminal world pose

2026-08-07 08:01:18 +08:00
parent 0bba8d7e61
commit e844fe00a8
6 changed files with 249 additions and 11 deletions
@@ -86,6 +86,50 @@ internal sealed class TrajectorySampleSchedule
    public IReadOnlyList<TrajectorySample> Samples { get; }
    public int TerminalAnchorSampleIndex { get; }

+    internal static bool ExceedsMaximumSampleCount(LongitudinalCandidate candidate, double outputTimeStepSeconds,
+        double holdDurationSeconds, EmLongitudinalMode mode, bool resampleMotion, int maximumSampleCount,
+        out int requiredSampleCount)
+    {
+        requiredSampleCount = 0;
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (maximumSampleCount < 1)
+            throw new ArgumentOutOfRangeException(nameof(maximumSampleCount));
+
+        if (resampleMotion)
+        {
+            double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
+            for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
+                sampleTime += outputTimeStepSeconds)
+            {
+                if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                    return true;
+            }
+            if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                return true;
+        }
+        else
+        {
+            for (int index = 0; index < candidate.KnotTimes.Count; index++)
+            {
+                if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                    return true;
+            }
+        }
+
+        if (mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return false;
+
+        double holdElapsed = 0d;
+        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
+        {
+            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
+            if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                return true;
+        }
+        return false;
+    }
+
    private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
        ref double previousPathS, LongitudinalCandidate candidate)
    {
@@ -97,6 +141,12 @@ internal sealed class TrajectorySampleSchedule
        previousPathS = sample.PathS;
    }

+    private static bool IncrementAndExceedsMaximum(ref int sampleCount, int maximumSampleCount)
+    {
+        checked { sampleCount++; }
+        return sampleCount > maximumSampleCount;
+    }
+
    private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval)
    {
        int lastKnot = candidate.KnotTimes.Count - 1;