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
    {