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
@@ -82,9 +82,15 @@ public sealed class EmPlanningService : IEmPlanningService
                return Failure(lateral.Status, request, lateral.FailureReason);
            EmitDebug(request, "LS optimization and validation succeeded");

+            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+                configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
+            LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
+                new LongitudinalPreviousTrajectorySeedBuilder().Build(
+                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
+                    segment.SegmentIndex, segment.Direction);
            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
-                Array.Empty<double>(), Array.Empty<double>());
+                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);