feat: separate rolling horizons from stop boundaries

2026-08-05 17:13:09 +08:00
parent e56220fc29
commit 21b20d09d4
9 changed files with 246 additions and 96 deletions
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 using MultiWheelC.TrajectoryPlanning.Utils;
@@ -142,6 +143,45 @@ public static class EmPlanningRequestValidator
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;

+        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
+                out _) ||
+            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
+                out _))
+        {
+            failureReason = "Configuration cannot construct the jerk-limited stopping model.";
+            return false;
+        }
+
+        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
+            longitudinal.MaximumReverseSpeedMetersPerSecond);
+        double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
+            maximumSpeed * scheduling.ReplanPeriodSeconds;
+        if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
+        {
+            failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
+                ";configured=" + scheduling.DistanceHorizonMeters;
+            return false;
+        }
+
+        try
+        {
+            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+                scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+            LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
+                scheduling.OutputTimeStepSeconds);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+
        return true;
    }