test: cover rolling-to-stop EM planning flow

ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs

@@ -7,23 +7,26 @@ namespace EMPlannerVerificationHost;
 
 internal static class EmFixtureFactory
 {
-    public static PathSmoothingResult CreateGearPairReferencePath()
+    public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
     {
+        if (firstSegmentLength <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
+        double midpoint = 0.5d * firstSegmentLength;
         var points = new List<SmoothedPathPoint>
         {
             Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
-            Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
+            Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
-            Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
+            Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
-            Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
+            Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
-            Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
+            Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
-            Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
+            Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
         };
         var segments = new List<SmoothedPathSegment>
         {
             new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
             new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
         };
-        var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
+        var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
         return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
             new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
     }

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -15,6 +15,7 @@ internal static class EmPlanningServiceChecks
     {
         VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
         VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
+        VerifiesServicePublishesRollingApproachAndExactStopModes();
         VerifiesRequestAndStateFailuresPublishNoTrajectory();
         VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
         VerifiesTimeoutFallbackAndCancellationSemantics();
@@ -50,6 +51,100 @@ internal static class EmPlanningServiceChecks
         VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
     }
 
+    private static void VerifiesServicePublishesRollingApproachAndExactStopModes()
+    {
+        EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
+            CreateReferencePath(TravelDirection.Forward, false, 10d), CreateMap(false, 12d));
+        ConfigureFiveMeterWindowAndTwoSecondHorizon(rollingRequest.Configuration);
+        EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            rollingRequest, CancellationToken.None);
+        VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "cycle 1 rolling");
+        Verification.Equal(EmLongitudinalMode.RollingContinuation,
+            rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
+        Verification.Equal(21, rolling.Trajectory.Points.Count, "rolling publishes the two-second ST knot count");
+        EmTrajectoryPoint rollingTerminal = rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1];
+        Verification.True(rollingTerminal.PathS < 5d, "two-second ST output remains inside the five-metre LS window");
+        Verification.True(rollingTerminal.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");
+
+        EmPlanningRequest approachRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
+            CreateReferencePath(TravelDirection.Forward, false, 4d), CreateMap(false, 5d));
+        EmPlanningResult approach = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            approachRequest, CancellationToken.None);
+        VerifySuccess(approach, approachRequest, EmTerminalType.Goal, "cycle 2 approach");
+        Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
+            approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");
+        Verification.True(approach.Trajectory.Points[approach.Trajectory.Points.Count - 1].SignedLongitudinalVelocity != 0d,
+            "approach has no synthetic stop tail");
+
+        EmPlanningRequest exactRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
+            CreateReferencePath(TravelDirection.Forward, false, 0.0075d));
+        ConfigureExactStopServiceScenario(exactRequest.Configuration);
+        EmPlanningResult exact = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            exactRequest, CancellationToken.None);
+        VerifySuccess(exact, exactRequest, EmTerminalType.Goal, "cycle 3 exact goal stop");
+        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
+            exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
+        AssertExactStopStabilization(exact.Trajectory, EmBoundaryType.Goal, "goal");
+
+        EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
+            EmFixtureFactory.CreateGearPairReferencePath(0.0075d));
+        ConfigureExactStopServiceScenario(gearRequest.Configuration);
+        EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            gearRequest, CancellationToken.None);
+        VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear-switch exact stop");
+        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
+            gear.Trajectory.Metadata.LongitudinalMode, "gear switch stops exactly");
+        AssertExactStopStabilization(gear.Trajectory, EmBoundaryType.GearSwitchApproach, "gear switch");
+        for (int index = 0; index < gear.Trajectory.Points.Count; index++)
+        {
+            Verification.Equal(TravelDirection.Forward, gear.Trajectory.Points[index].Direction,
+                "gear-switch publication excludes the next direction point " + index);
+        }
+    }
+
+    private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
+    {
+        configuration.Scheduling.DistanceHorizonMeters = 5d;
+        configuration.Scheduling.TimeHorizonSeconds = 2d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
+    }
+
+    private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
+    {
+        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
+        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
+    }
+
+    private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
+    {
+        int exactAnchor = -1;
+        for (int index = 0; index < trajectory.Points.Count; index++)
+        {
+            if (trajectory.Points[index].BoundaryType == boundaryType)
+            {
+                exactAnchor = index;
+                break;
+            }
+        }
+        Verification.True(exactAnchor >= 0, name + " has a real boundary anchor");
+        Verification.NearlyEqual(0d, trajectory.Points[exactAnchor].SignedLongitudinalVelocity,
+            name + " speed is zero");
+        Verification.True(trajectory.Points.Count > exactAnchor + 1,
+            name + " anchor is followed by a QP stabilization point");
+        Verification.NearlyEqual(trajectory.Points[exactAnchor].PathS, trajectory.Points[exactAnchor + 1].PathS,
+            name + " stabilization keeps the stop position");
+        Verification.NearlyEqual(0d, trajectory.Points[exactAnchor + 1].SignedLongitudinalVelocity,
+            name + " stabilization speed is zero");
+    }
+
     private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
     {
         EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
@@ -300,7 +395,7 @@ internal static class EmPlanningServiceChecks
             new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
     }
 
-    private static PlanningGridMap CreateMap(bool blockStart)
+    private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d)
     {
         IMapObstacleSource[] sources = blockStart
             ? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
@@ -308,7 +403,7 @@ internal static class EmPlanningServiceChecks
             : Array.Empty<IMapObstacleSource>();
         PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
         {
-            Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f),
+            Bounds = new MapBoundsMm((float)(-1000d * halfExtentMeters), (float)(1000d * halfExtentMeters), -1000f, 1000f),
             ResolutionMm = 20f,
             ObstacleSources = sources,
             AllowExplicitEmptyMap = !blockStart,

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -28,9 +28,11 @@ internal static class LongitudinalIntegrationChecks
     private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.DistanceHorizonMeters = 5d;
         configuration.Scheduling.TimeHorizonSeconds = 2d;
         configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
         configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
         LateralPath path = new LateralPath(new[]
         {
             Point(0d, 0d, 0d),
@@ -46,9 +48,10 @@ internal static class LongitudinalIntegrationChecks
             CancellationToken.None);
 
         Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
-            "rolling seed remains a strict timeout fallback");
+            "rolling seed remains a strict timeout fallback: " + result.FailureReason);
         LongitudinalCandidate candidate = result.Candidate ??
             throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
+        Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots");
         Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
             "two-second ST does not consume the five-metre LS window");
         Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,