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
@@ -11,7 +11,8 @@ public sealed class LongitudinalPlanningInput
    private const double PathSTolerance = 1e-12d;

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
-        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
@@ -25,6 +26,12 @@ public sealed class LongitudinalPlanningInput
            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
+            throw new ArgumentOutOfRangeException(nameof(mode));
+        if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
+        if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));

@@ -33,6 +40,7 @@ public sealed class LongitudinalPlanningInput
        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
        TerminalType = terminalType;
+        Mode = mode;
        Configuration = configuration.Copy();
        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
@@ -42,6 +50,17 @@ public sealed class LongitudinalPlanningInput
                nameof(previousProgressSpeedMetersPerSecond));
    }

+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType,
+            terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
    public LateralPath Path { get; }

    public TravelDirection Direction { get; }
@@ -52,13 +71,33 @@ public sealed class LongitudinalPlanningInput

    public EmTerminalType TerminalType { get; }

+    public EmLongitudinalMode Mode { get; }
+
    public EmPlannerConfiguration Configuration { get; }

    public IReadOnlyList<double> PreviousPathS { get; }

    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }

-    public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+    public double PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } }
+
+    public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } }
+
+    public EmBoundaryType StopBoundaryType
+    {
+        get
+        {
+            return TerminalType == EmTerminalType.Goal
+                ? EmBoundaryType.Goal
+                : TerminalType == EmTerminalType.GearSwitch
+                    ? EmBoundaryType.GearSwitchApproach
+                    : EmBoundaryType.None;
+        }
+    }
+
+    public double TerminalPathS { get { return PathUpperBoundS; } }

    public double DirectionMaximumSpeedMetersPerSecond
    {
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public static class LongitudinalTerminalSchedule
+{
+    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
+        double minimumStabilizationDurationSeconds)
+    {
+        if (knotTimes == null || knotTimes.Count < 3 ||
+            double.IsNaN(minimumStabilizationDurationSeconds) ||
+            double.IsInfinity(minimumStabilizationDurationSeconds) ||
+            minimumStabilizationDurationSeconds <= 0d)
+        {
+            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
+        }
+
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < knotTimes.Count; index++)
+        {
+            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
+                knotTimes[index] <= previous)
+            {
+                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
+            }
+            previous = knotTimes[index];
+        }
+
+        double finalTime = knotTimes[knotTimes.Count - 1];
+        for (int index = knotTimes.Count - 2; index >= 1; index--)
+        {
+            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
+                return index;
+        }
+
+        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
+    }
+}