feat: select complete EM direction segments

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -55,7 +55,8 @@ public sealed class EmPlanningService : IEmPlanningService
             double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
             var horizonSelector = new PlanningHorizonSelector();
             EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
-                initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
+                initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
+                out string horizonReason);
             if (horizonStatus != EmPlanningStatus.Success)
                 return Failure(horizonStatus, request, horizonReason);
             ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
@@ -212,6 +213,6 @@ public sealed class EmPlanningService : IEmPlanningService
             ";reference=" + (request.ReferencePathId ?? string.Empty) +
             ";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
             ";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
-            ";segment=" + request.SegmentIndex;
+            ";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
     }
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -40,7 +40,8 @@ public sealed class PlanningHorizonSelector
 
     public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
         double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
-        EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
+        EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
+        out string failureReason)
     {
         selection = null;
         failureReason = string.Empty;
@@ -48,7 +49,8 @@ public sealed class PlanningHorizonSelector
             !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
             currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
             !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
-            !IsFinite(initialAccelerationMetersPerSecondSquared))
+            !IsFinite(initialAccelerationMetersPerSecondSquared) ||
+            !Enum.IsDefined(typeof(EmPlanningScope), planningScope))
         {
             failureReason = "Planning horizon inputs are invalid.";
             return EmPlanningStatus.InvalidInput;
@@ -90,6 +92,20 @@ public sealed class PlanningHorizonSelector
             return EmPlanningStatus.StoppingDistanceInsufficient;
         }
 
+        if (planningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            EmBoundaryType boundary = segment.EndBoundary.BoundaryType;
+            if (!IsStopBoundary(boundary))
+            {
+                failureReason = "A full direction segment must end at Goal or GearSwitch.";
+                return EmPlanningStatus.InvalidReferencePath;
+            }
+            selection = new PlanningHorizonSelection(segment.LengthMeters, boundary,
+                ToTerminalType(boundary), EmLongitudinalMode.ExactStopAtBoundary,
+                segment.LengthMeters, true);
+            return EmPlanningStatus.Success;
+        }
+
         double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
         bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
         bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);

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");