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
@@ -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)