feat: reuse prior trajectory in longitudinal planning

2026-08-05 20:32:40 +08:00
parent 4159ae0bc1
commit 6cfbaf61f5
4 changed files with 413 additions and 13 deletions
@@ -13,6 +13,7 @@ internal static class EmPlanningServiceChecks
 {
    public static void Run()
    {
+        VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
        VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
        VerifiesRequestAndStateFailuresPublishNoTrajectory();
        VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
@@ -31,7 +32,7 @@ internal static class EmPlanningServiceChecks
        VerifySameTrajectory(firstForward, secondForward, "forward deterministic result");
        Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged");

-        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, 0d, false, false);
+        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, -0.01d, false, false);
        EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
            CancellationToken.None);
        VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
@@ -70,7 +71,7 @@ internal static class EmPlanningServiceChecks
    private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory()
    {
        EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
-        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
+        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(3d, 0d, 0d), 0d, null,
            projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
            CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");
@@ -80,15 +81,20 @@ internal static class EmPlanningServiceChecks
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
            CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");

-        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false);
+        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false,
+            referencePath: CreateReferencePath(TravelDirection.Forward, false, 0.0055d));
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure,
            CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient");

        EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false);
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular,
            CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible");
-        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular,
-            CancellationToken.None), EmPlanningStatus.LongitudinalInfeasible, "longitudinal infeasible");
+        EmPlanningResult longitudinalFallback = new EmPlanningService(
+            new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, longitudinalFallback.Status,
+            "longitudinal infeasible uses the validated fallback seed");
+        Verification.True(longitudinalFallback.Trajectory != null,
+            "longitudinal fallback still publishes a complete trajectory");
        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
            CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
    }
@@ -127,6 +133,56 @@ internal static class EmPlanningServiceChecks
        VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
    }

+    private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
+
+        var withoutPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult withoutPrevious = new EmPlanningService(withoutPreviousSolver).Plan(request, CancellationToken.None);
+        VerifySuccess(withoutPrevious, request, EmTerminalType.Goal, "no previous longitudinal seed");
+
+        EmTrajectory validPrevious = CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Forward,
+            request.SegmentIndex);
+        EmPlanningRequest withPreviousRequest = ReplacePreviousTrajectory(request, validPrevious);
+        var withPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult withPrevious = new EmPlanningService(withPreviousSolver).Plan(withPreviousRequest,
+            CancellationToken.None);
+        VerifySuccess(withPrevious, withPreviousRequest, EmTerminalType.Goal, "valid previous longitudinal seed");
+
+        QuadraticProgram withoutPreviousProblem = withoutPreviousSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The no-seed longitudinal QP was not captured.");
+        QuadraticProgram withPreviousProblem = withPreviousSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The seeded longitudinal QP was not captured.");
+        var layout = new LongitudinalVariableLayout((withPreviousProblem.VariableCount + 1) / 4);
+        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)) >
+            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            "valid previous seed adds a nonzero previous-S soft-reference Hessian term");
+        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)) >
+            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            "valid previous seed adds a nonzero previous-U soft-reference Hessian term");
+        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.S(1)] -
+            withoutPreviousProblem.LinearCost[layout.S(1)]) > 1e-12d,
+            "valid previous seed adds a nonzero previous-S soft-reference linear term");
+        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.U(1)] -
+            withoutPreviousProblem.LinearCost[layout.U(1)]) > 1e-12d,
+            "valid previous seed adds a nonzero previous-U soft-reference linear term");
+
+        EmPlanningRequest incompatibleRequest = ReplacePreviousTrajectory(request,
+            CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Reverse, request.SegmentIndex));
+        var incompatibleSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult incompatible = new EmPlanningService(incompatibleSolver).Plan(incompatibleRequest,
+            CancellationToken.None);
+        VerifySuccess(incompatible, incompatibleRequest, EmTerminalType.Goal, "incompatible previous seed");
+        QuadraticProgram incompatibleProblem = incompatibleSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The incompatible-seed longitudinal QP was not captured.");
+        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            MatrixValue(incompatibleProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            "incompatible previous seed omits previous-S soft-reference term");
+        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            MatrixValue(incompatibleProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            "incompatible previous seed omits previous-U soft-reference term");
+    }
+
    private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
        string name)
    {
@@ -170,7 +226,7 @@ internal static class EmPlanningServiceChecks
        configuration.Solver.MaximumOuterIterations = 2;
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        if (rolling)
-            configuration.Scheduling.DistanceHorizonMeters = 0.003d;
+            configuration.Scheduling.DistanceHorizonMeters = 0.30d;
        return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
            new VehicleParameters
            {
@@ -191,21 +247,55 @@ internal static class EmPlanningServiceChecks
            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
    }

-    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch)
+    private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
    {
-        double endX = direction == TravelDirection.Forward ? 0.0055d : -0.0055d;
+        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);
+    }
+
+    private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
+        TravelDirection direction, int segmentIndex)
+    {
+        var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
+            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation);
+        double sign = direction == TravelDirection.Forward ? 1d : -1d;
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 0d, 0d, segmentIndex, 0.001d, 0.001d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 6d, 0d, segmentIndex, 0.001d, 0.001d,
+                direction, EmBoundaryType.None, 0d, 0d),
+        });
+    }
+
+    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
+    {
+        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+        {
+            if (matrix.RowIndices[index] == row)
+                return matrix.Values[index];
+        }
+        return 0d;
+    }
+
+    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch,
+        double lengthMeters = 2d)
+    {
+        double endX = direction == TravelDirection.Forward ? lengthMeters : -lengthMeters;
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false,
                SmoothedPathPointSource.Anchor),
-            new SmoothedPathPoint(endX, 0d, 0d, 0d, 0.0055d, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
+            new SmoothedPathPoint(endX, 0d, 0d, 0d, lengthMeters, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
                endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
        };
        var segments = new List<SmoothedPathSegment>
        {
            new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch),
        };
-        var metrics = new PathQualityMetrics(true, 0.0055d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
+        var metrics = new PathQualityMetrics(true, lengthMeters, 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>());
    }
@@ -218,7 +308,7 @@ internal static class EmPlanningServiceChecks
            : Array.Empty<IMapObstacleSource>();
        PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
        {
-            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f),
            ResolutionMm = 20f,
            ObstacleSources = sources,
            AllowExplicitEmptyMap = !blockStart,
@@ -245,6 +335,8 @@ internal static class EmPlanningServiceChecks
        private readonly PlanningGridMap? mapToCorrupt;
        private int longitudinalCallCount;

+        public QuadraticProgram? LastLongitudinalProblem { get; private set; }
+
        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
        {
            this.mode = mode;
@@ -265,6 +357,7 @@ internal static class EmPlanningServiceChecks
                    return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
                return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
            }
+            LastLongitudinalProblem = problem;
            if (mode == PipelineSolverMode.LongitudinalInfeasible)
                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
@@ -272,7 +365,7 @@ internal static class EmPlanningServiceChecks
            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
            longitudinalCallCount++;
-            return Result(QpSolveStatus.Solved, CreateStrictLongitudinalPrimal(problem));
+            return Result(QpSolveStatus.Solved, warmStart);
        }

        private static void CorruptMapAtOrigin(PlanningGridMap? map)
@@ -20,6 +20,7 @@ internal static class LongitudinalModelChecks
        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
        VerifiesModeSpecificSolutionValidation();
+        VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
    }

    private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
@@ -454,6 +455,34 @@ internal static class LongitudinalModelChecks
            "approach failure identifies the jerk-limited stoppable set");
    }

+    private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
+    {
+        LateralPath path = CreateStraightPath(1d);
+        DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
+        EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
+        var builder = new LongitudinalPreviousTrajectorySeedBuilder();
+        LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);
+
+        Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
+        Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
+        Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
+        Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
+            "previous seed progress is monotone");
+        Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
+            "previous seed uses absolute progress speed");
+
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
+            "different direction returns an empty seed");
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
+            "different segment returns an empty seed");
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
+            new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
+            "out-of-range absolute sampling returns an empty seed");
+    }
+
    private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
    {
        var points = new List<LateralPathPoint>(fixtures.Count);
@@ -466,6 +495,28 @@ internal static class LongitudinalModelChecks
        return new LateralPath(points, true);
    }

+    private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
+        int segmentIndex)
+    {
+        var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
+            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation);
+        double sign = direction == TravelDirection.Forward ? 1d : -1d;
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
+                direction, EmBoundaryType.None, 0d, 0d),
+        });
+    }
+
    private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
    {
        var points = new List<SmoothedPathPoint>