feat: define full-direction EM planning scope

2026-08-06 20:48:44 +08:00
parent fcf7df17d5
commit f4e89b4b4f
20 changed files with 183 additions and 33 deletions
@@ -296,7 +296,7 @@ internal static class CoordinatorChecks
        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L,
            string.Empty, 8, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop
                ? EmLongitudinalMode.RollingContinuation
-                : EmLongitudinalMode.ExactStopAtBoundary);
+                : EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction,
@@ -356,7 +356,7 @@ internal static class CoordinatorChecks
        EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None;
        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L,
            string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
-            EmLongitudinalMode.RollingContinuation);
+            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d,
@@ -403,7 +403,8 @@ internal static class CoordinatorChecks
        configuration ??= EmPlannerConfiguration.CreateDefault();
        state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
        var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now,
-            outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse);
+            outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse,
+            EmPlanningScope.RollingHorizon);
        return new PlanningCycleInput(request, now);
    }

@@ -480,7 +481,7 @@ internal static class CoordinatorChecks
            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
-                EmLongitudinalMode.RollingContinuation);
+                EmLongitudinalMode.RollingContinuation, request.PlanningScope);
            var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d,
                TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d);
            return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty);
@@ -320,6 +320,10 @@ internal static class EmPlanningServiceChecks
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Solver.MaximumOuterIterations = 2;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.20d;
+        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.20d;
+        configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond = 0.20d;
        if (rolling)
            configuration.Scheduling.DistanceHorizonMeters = 0.30d;
        return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
@@ -332,21 +336,23 @@ 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);
+            EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon);
    }

    private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
    {
        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
            source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
-            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
+            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
+            source.PlanningScope);
    }

    private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
    {
        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState,
            source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
-            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
+            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
+            source.PlanningScope);
    }

    private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
@@ -354,7 +360,7 @@ internal static class EmPlanningServiceChecks
    {
        var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
-            EmLongitudinalMode.RollingContinuation);
+            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        double sign = direction == TravelDirection.Forward ? 1d : -1d;
        return new EmTrajectory(metadata, new[]
        {
@@ -255,7 +255,7 @@ internal static class ExecutorChecks
            "executor-reference", 60L, string.Empty, 4, direction, terminalType,
            terminalType == EmTerminalType.RollingSafetyStop
                ? EmLongitudinalMode.RollingContinuation
-                : EmLongitudinalMode.ExactStopAtBoundary);
+                : EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d),
@@ -269,7 +269,7 @@ internal static class ExecutorChecks
    {
        var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
            "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal,
-            EmLongitudinalMode.ExactStopAtBoundary);
+            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,
@@ -12,6 +12,7 @@ internal static class FoundationChecks
    {
        VerifyContracts();
        VerifyConfigurationAndRequestValidation();
+        VerifyFullDirectionScopeContractsConfigurationAndValidation();
    }

    private static void VerifyContracts()
@@ -63,7 +64,8 @@ internal static class FoundationChecks
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period");
        EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon");
-        EMPlannerVerificationHost.Verification.NearlyEqual(5d, configuration.Scheduling.DistanceHorizonMeters, "distance horizon");
+        EMPlannerVerificationHost.Verification.True(configuration.Scheduling.DistanceHorizonMeters > 0d,
+            "distance horizon remains positive");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead");
@@ -84,8 +86,8 @@ internal static class FoundationChecks
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative");

-        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
-        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk");
@@ -150,6 +152,86 @@ internal static class FoundationChecks
        EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message");
    }

+    private static void VerifyFullDirectionScopeContractsConfigurationAndValidation()
+    {
+        EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault();
+        EmPlanningRequest fullRequest = CreateValidRequest(defaults, EmPlanningScope.FullDirectionSegment);
+        EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullRequest.PlanningScope,
+            "full request freezes its scope");
+        var fullMetadata = new EmTrajectoryMetadata("full-scope", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch,
+            1L, "reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal,
+            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
+        EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullMetadata.PlanningScope,
+            "metadata freezes its scope");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1d,
+            defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond, "forward desired speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d,
+            defaults.Longitudinal.DesiredReverseSpeedMetersPerSecond, "reverse desired speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1d,
+            defaults.Longitudinal.MaximumForwardSpeedMetersPerSecond, "forward hard limit");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d,
+            defaults.Longitudinal.MaximumReverseSpeedMetersPerSecond, "reverse hard limit");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d,
+            defaults.Scheduling.MaximumOptimizationTimeStepSeconds, "adaptive maximum dt");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.10d,
+            defaults.Scheduling.MaximumOptimizationSpatialStepMeters, "adaptive maximum ds");
+        EMPlannerVerificationHost.Verification.Equal(401, defaults.Scheduling.MaximumOptimizationKnotCount,
+            "optimization knot cap");
+        EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount,
+            "publication sample cap");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
+            defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance");
+        EMPlannerVerificationHost.Verification.NearlyEqual(5d * Math.PI / 180d,
+            defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance");
+        EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress,
+            "no-progress status exists");
+        EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.TerminalPoseMismatch,
+            EmPlanningStatus.TerminalPoseMismatch, "terminal-pose status exists");
+        EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded,
+            EmPlanningStatus.FullSegmentResourceLimitExceeded, "resource-limit status exists");
+
+        EmPlannerConfiguration copied = (EmPlannerConfiguration)typeof(EmPlannerConfiguration)
+            .GetMethod("Copy", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic)!
+            .Invoke(defaults, null)!;
+        defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.25d;
+        defaults.Scheduling.MaximumOptimizationKnotCount = 3;
+        defaults.Validation.TerminalPositionToleranceMeters = 0.01d;
+        EMPlannerVerificationHost.Verification.NearlyEqual(1d,
+            copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed");
+        EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount,
+            "copy keeps knot cap");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
+            copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance");
+
+        EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault();
+        invalidKnotCap.Scheduling.MaximumOptimizationKnotCount = 2;
+        AssertStatus(EmPlanningStatus.InvalidInput,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(invalidKnotCap, EmPlanningScope.FullDirectionSegment)),
+            "invalid optimization knot cap");
+        EmPlannerConfiguration invalidDesiredSpeed = EmPlannerConfiguration.CreateDefault();
+        invalidDesiredSpeed.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1.01d;
+        AssertStatus(EmPlanningStatus.InvalidInput,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(invalidDesiredSpeed, EmPlanningScope.FullDirectionSegment)),
+            "desired speed above hard limit");
+        EmPlannerConfiguration invalidYawTolerance = EmPlannerConfiguration.CreateDefault();
+        invalidYawTolerance.Validation.TerminalYawToleranceRadians = Math.PI + 0.01d;
+        AssertStatus(EmPlanningStatus.InvalidInput,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(invalidYawTolerance, EmPlanningScope.FullDirectionSegment)),
+            "terminal yaw tolerance above pi");
+
+        EmPlannerConfiguration shortLegacyWindow = EmPlannerConfiguration.CreateDefault();
+        shortLegacyWindow.Scheduling.DistanceHorizonMeters = 0.01d;
+        AssertStatus(EmPlanningStatus.Success,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.FullDirectionSegment)),
+            "full scope ignores legacy stopping-preview truncation");
+        AssertStatus(EmPlanningStatus.InvalidInput,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.RollingHorizon)),
+            "rolling scope retains legacy stopping-preview validation");
+        AssertStatus(EmPlanningStatus.InvalidInput,
+            EmPlanningRequestValidator.Validate(CreateValidRequest(EmPlannerConfiguration.CreateDefault(),
+                (EmPlanningScope)99)), "undefined planning scope");
+    }
+
    private static void VerifyLateralWeights(LateralWeights weights)
    {
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight");
@@ -176,7 +258,8 @@ internal static class FoundationChecks
        EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name);
    }

-    private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration)
+    private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration,
+        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
    {
        DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
        return CreateRequest(
@@ -186,7 +269,8 @@ internal static class FoundationChecks
            CreateState(0d, 9L, requestedAtUtc),
            configuration,
            0,
-            EmMotionModel.NonholonomicForwardReverse);
+            EmMotionModel.NonholonomicForwardReverse,
+            planningScope);
    }

    private static EmPlanningRequest CreateRequest(
@@ -196,7 +280,8 @@ internal static class FoundationChecks
        VehicleMotionState vehicleState,
        EmPlannerConfiguration configuration,
        int segmentIndex,
-        EmMotionModel motionModel)
+        EmMotionModel motionModel,
+        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
    {
        DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
        return new EmPlanningRequest(
@@ -212,7 +297,8 @@ internal static class FoundationChecks
            "trajectory-1",
            "reference-1",
            string.Empty,
-            motionModel);
+            motionModel,
+            planningScope);
    }

    private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc)
@@ -500,7 +500,7 @@ internal static class LongitudinalModelChecks
    {
        var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
-            EmLongitudinalMode.RollingContinuation);
+            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        double sign = direction == TravelDirection.Forward ? 1d : -1d;
        return new EmTrajectory(metadata, new[]
        {
@@ -403,6 +403,7 @@ internal static class TrajectoryChecks
        EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
    {
        return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
-            "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode);
+            "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode,
+            EmPlanningScope.RollingHorizon);
    }
 }
@@ -901,7 +901,7 @@ internal static class TrajectoryObservationChecks
    {
        var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType,
-            EmLongitudinalMode.RollingContinuation);
+            EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
@@ -944,7 +944,7 @@ internal static class TrajectoryObservationChecks
            "active-segment-reference", 1L, string.Empty, segmentIndex, direction, terminalType,
            terminalType == EmTerminalType.GearSwitch
                ? EmLongitudinalMode.ExactStopAtBoundary
-                : EmLongitudinalMode.RollingContinuation);
+                : EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(x, 0d, 0d, 0d, 0d, 0d, segmentIndex, 1d, 1d,
@@ -189,7 +189,7 @@ internal static class TrajectoryObservationSegmentChecks
    {
        var metadata = new EmTrajectoryMetadata("gear-terminal-" + effectiveAtUtc.Ticks, effectiveAtUtc,
            effectiveAtUtc, 1L, "segment-check", 1L, string.Empty, 0, direction, terminalType,
-            EmLongitudinalMode.ExactStopAtBoundary);
+            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(1d, 0d, 0d, 0d, finalTimeFromStart, 0d, 0, 1d, 1d,
@@ -470,7 +470,7 @@ internal static class TrajectoryObservationVisualizationChecks
                0.2d + t, 0.3d + t));
        }
        return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L,
-            string.Empty, 0, direction, terminal, mode), points);
+            string.Empty, 0, direction, terminal, mode, EmPlanningScope.RollingHorizon), points);
    }

    private sealed class FixedPlanningService : IEmPlanningService