feat: select complete EM direction segments

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -16,6 +16,7 @@ internal static class EmPlanningServiceChecks
         VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
         VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
         VerifiesServicePublishesRollingApproachAndExactStopModes();
+        VerifiesFullScopePublishesItsRequestScope();
         VerifiesRequestAndStateFailuresPublishNoTrajectory();
         VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
         VerifiesTimeoutFallbackAndCancellationSemantics();
@@ -102,6 +103,22 @@ internal static class EmPlanningServiceChecks
         }
     }
 
+    private static void VerifiesFullScopePublishesItsRequestScope()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
+            CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
+            EmPlanningScope.FullDirectionSegment);
+        ConfigureExactStopServiceScenario(request.Configuration);
+        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            request, CancellationToken.None);
+        VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
+        Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
+            "service metadata preserves full scope");
+        Verification.NearlyEqual(0d, result.Trajectory.Points[0].PathS, "full publication starts at projection");
+        Verification.NearlyEqual(0.0075d, result.Trajectory.Points[result.Trajectory.Points.Count - 1].PathS,
+            "full publication reaches the real segment boundary");
+    }
+
     private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
     {
         configuration.Scheduling.DistanceHorizonMeters = 5d;
@@ -314,7 +331,8 @@ internal static class EmPlanningServiceChecks
     }
 
     private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
-        bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null)
+        bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null,
+        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
     {
         DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -336,7 +354,7 @@ internal static class EmPlanningServiceChecks
             },
             new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
             null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
-            EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon);
+            EmMotionModel.NonholonomicForwardReverse, planningScope);
     }
 
     private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -18,6 +18,7 @@ internal static class LongitudinalModelChecks
         VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
         VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
         VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
+        VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
         VerifiesModeSpecificSolutionValidation();
         VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
@@ -107,7 +108,7 @@ internal static class LongitudinalModelChecks
             out PathSpeedLimit directionEnvelope, out string directionFailureReason);
         Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
             directionFailureReason);
-        Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
+        Verification.NearlyEqual(1d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
             "default direction speed limit");
 
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -235,7 +236,7 @@ internal static class LongitudinalModelChecks
 
         DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
         EmPlanningStatus status = new PlanningHorizonSelector().Select(
-            longSegment, 0d, 0d, 0d, configuration,
+            longSegment, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
             out PlanningHorizonSelection rolling, out string failure);
         Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
         Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
@@ -245,7 +246,7 @@ internal static class LongitudinalModelChecks
 
         DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
         status = new PlanningHorizonSelector().Select(
-            visibleButFar, 0d, 0d, 0d, configuration,
+            visibleButFar, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
             out PlanningHorizonSelection approach, out failure);
         Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
         Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
@@ -253,7 +254,7 @@ internal static class LongitudinalModelChecks
 
         DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
         status = new PlanningHorizonSelector().Select(
-            reachableGoal, 0d, 0d, 0d, configuration,
+            reachableGoal, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
             out PlanningHorizonSelection exact, out failure);
         Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
         Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
@@ -269,6 +270,33 @@ internal static class LongitudinalModelChecks
             "short final interval moves the stop anchor earlier");
     }
 
+    private static void VerifiesFullDirectionScopeSelectsActualSegmentBoundary()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.DistanceHorizonMeters = 1d;
+        DirectionSegmentView tenMeterGoal = CreateSegment(10d, EmBoundaryType.Goal);
+        var selector = new PlanningHorizonSelector();
+        EmPlanningStatus status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
+            EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection full, out string failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "full selection: " + failure);
+        Verification.NearlyEqual(10d, full.WindowEndReferenceS, "full selection reaches actual segment end");
+        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, full.LongitudinalMode,
+            "full selection stops at its real boundary");
+        Verification.True(full.HasStopBoundary, "full selection retains the real stop boundary");
+
+        status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
+            EmPlanningScope.RollingHorizon, configuration, out PlanningHorizonSelection rolling, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
+        Verification.NearlyEqual(4d, rolling.WindowEndReferenceS, "rolling selection retains the one-metre window");
+
+        DirectionSegmentView gearSwitch = CreateSegment(10d, EmBoundaryType.GearSwitchApproach);
+        status = selector.Select(gearSwitch, 3d, 0d, 0d,
+            EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection gear, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "gear full selection: " + failure);
+        Verification.Equal(EmTerminalType.GearSwitch, gear.TerminalType, "gear full selection preserves switch terminal");
+        Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
+    }
+
     private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
     {
         var layout = new LongitudinalVariableLayout(5);

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -135,7 +135,7 @@ internal static class TrajectoryChecks
             EmTrajectoryValidationFailure.WorldVelocityMismatch, 1, "world velocity");
         AssertRejected(context, CorruptDouble(valid, 1, "YawRate", valid.Points[1].YawRate + 0.01d),
             EmTrajectoryValidationFailure.YawRateMismatch, 1, "yaw rate");
-        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.30d)),
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 1.10d)),
             EmTrajectoryValidationFailure.SpeedLimitExceeded, 1, "speed limit");
         AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], signedSpeed: 0.19d)),
             EmTrajectoryValidationFailure.AccelerationLimitExceeded, 2, "acceleration limit");