feat: keep rolling speed envelopes open

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -12,10 +12,11 @@ internal static class LongitudinalModelChecks
     {
         VerifiesJerkLimitedStoppingProfileEndsAtRest();
         VerifiesStoppedReachabilityUsesTheSameJerkModel();
+        VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
         VerifiesFinitePathSIndexedSpeedEnvelope();
         VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
-        VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
-        VerifiesStoppingPrecheckBeforeQpAssembly();
+        VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
+        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
         VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
     }
@@ -56,6 +57,40 @@ internal static class LongitudinalModelChecks
             "distance inversion stays inside the direction speed range");
     }
 
+    private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1d, 1d, 0d, 0d),
+        });
+        var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
+            rolling, out PathSpeedLimit envelope, out string failure);
+
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
+        Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
+            "rolling window end keeps a nonzero speed allowance");
+        Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
+        Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
+            "rolling envelope reports its PathS upper bound");
+
+        var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.Goal,
+            EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
+        Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
+        Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
+            "approach stop boundary has an exact zero stopping limit");
+    }
+
     private static void VerifiesFinitePathSIndexedSpeedEnvelope()
     {
         LateralPath directionPath = CreatePath(new[]
@@ -96,8 +131,8 @@ internal static class LongitudinalModelChecks
         Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
         Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
             double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
-            "stopping speed limit");
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
+            "stopping speed limit uses the complete jerk-limited model");
         Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
             "combined limit chooses the finite minimum");
         double interpolationQueryPathS = 1.013d;
@@ -133,11 +168,11 @@ internal static class LongitudinalModelChecks
             out string failureReason);
         Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
         Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (2d - 1.5d)), envelope.MaximumSpeedAt(1.5d),
-            "refined stopping envelope avoids a sparse terminal chord");
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
+            "refined stopping envelope uses the complete jerk-limited stopping cap");
     }
 
-    private static void VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations()
+    private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
         configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
@@ -153,22 +188,16 @@ internal static class LongitudinalModelChecks
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
-        Verification.Equal(EmPlanningStatus.Success, status, "discrete stopping-tail status: " + failureReason);
-        double timeStep = configuration.Scheduling.OutputTimeStepSeconds;
-        double deceleration = configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
-        double firstTailDistance = 0.5d * deceleration * timeStep * timeStep;
-        double firstTailPathS = input.TerminalPathS - firstTailDistance;
-        Verification.NearlyEqual(deceleration * timeStep, envelope.StoppingLimitAt(firstTailPathS),
-            "first stopping-tail station matches one discrete deceleration time step");
-        double jerk = configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
-        double firstJerkTailDistance = jerk * timeStep * timeStep * timeStep / 6d;
-        double firstJerkTailPathS = input.TerminalPathS - firstJerkTailDistance;
-        Verification.NearlyEqual(Math.Sqrt(2d * deceleration * firstJerkTailDistance),
-            envelope.StoppingLimitAt(firstJerkTailPathS),
-            "first stopping-tail station matches one discrete jerk-release time step");
+        Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
+        double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
+            envelope.StoppingLimitAt(nearBoundaryPathS),
+            "near-boundary speed cap uses jerk-limited distance inversion");
+        Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
+            "real stop boundary keeps an exact zero stopping cap");
     }
 
-    private static void VerifiesStoppingPrecheckBeforeQpAssembly()
+    private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
         LateralPath shortPath = CreatePath(new[]
@@ -176,14 +205,22 @@ internal static class LongitudinalModelChecks
             new PathFixture(0d, 0d, 0d, 0d),
             new PathFixture(100d, 0.01d, 0d, 0d),
         });
-        var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+        var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
             EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
             Array.Empty<double>(), Array.Empty<double>());
 
-        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
             out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status,
+            "rolling windows do not require a stop inside their local PathS extent: " + failureReason);
+        Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");
+
+        var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
         Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
-            "jerk/deceleration stopping precheck status");
+            "real stop-boundary jerk/deceleration stopping precheck status");
         Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
         Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
     }
@@ -375,6 +412,17 @@ internal static class LongitudinalModelChecks
         return configuration;
     }
 
+    private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
+    {
+        LongitudinalConfiguration limits = input.Configuration.Longitudinal;
+        return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+            Math.Max(0d, input.StopBoundaryPathS - pathS),
+            limits.MaximumAccelerationMetersPerSecondSquared,
+            limits.MaximumDecelerationMetersPerSecondSquared,
+            limits.MaximumJerkMetersPerSecondCubed,
+            input.DirectionMaximumSpeedMetersPerSecond);
+    }
+
     private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
     {
         for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)