feat: validate EM terminal world pose

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs

@@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler
 {
     private readonly double outputTimeStepSeconds;
     private readonly double zeroSpeedHoldSeconds;
+    private readonly int maximumPublishedSampleCount;
 
     public EmTrajectoryAssembler()
         : this(EmPlannerConfiguration.CreateDefault())
@@ -21,8 +22,9 @@ public sealed class EmTrajectoryAssembler
             throw new ArgumentNullException(nameof(configuration));
         outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
         zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
+        maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
         if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
-            zeroSpeedHoldSeconds < 0d)
+            zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
         {
             throw new ArgumentOutOfRangeException(nameof(configuration));
         }
@@ -30,6 +32,18 @@ public sealed class EmTrajectoryAssembler
 
     public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
     {
+        EmPlanningStatus status = TryAssemble(path, longitudinal, metadata, out EmTrajectory trajectory,
+            out string failureReason);
+        if (status != EmPlanningStatus.Success)
+            throw new ArgumentException(failureReason, nameof(longitudinal));
+        return trajectory;
+    }
+
+    public EmPlanningStatus TryAssemble(LateralPath path, LongitudinalPlanningResult longitudinal,
+        EmTrajectoryMetadata metadata, out EmTrajectory trajectory, out string failureReason)
+    {
+        trajectory = null;
+        failureReason = string.Empty;
         if (longitudinal == null || longitudinal.Candidate == null ||
             (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
         {
@@ -40,8 +54,17 @@ public sealed class EmTrajectoryAssembler
 
         var interpolator = new LateralPathInterpolator(path);
         bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
-        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
-            isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
+        double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds;
+        if (TrajectorySampleSchedule.ExceedsMaximumSampleCount(longitudinal.Candidate, outputTimeStepSeconds,
+                holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, maximumPublishedSampleCount,
+                out int requiredSampleCount))
+        {
+            failureReason = "FullSegmentResourceLimitExceeded: MaximumPublishedSampleCount=" +
+                maximumPublishedSampleCount + ";required-at-least=" + requiredSampleCount;
+            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
+        }
+        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
+            metadata.LongitudinalMode, isFullDirectionSegment);
         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;
@@ -62,7 +85,8 @@ public sealed class EmTrajectoryAssembler
                 boundaryType, sample.Acceleration, sample.Jerk));
         }
 
-        return new EmTrajectory(metadata, points);
+        trajectory = new EmTrajectory(metadata, points);
+        return EmPlanningStatus.Success;
     }
 
     private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

@@ -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;