chore: save current workspace progress

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
@@ -43,11 +44,19 @@ public sealed class EmPlanningService : IEmPlanningService
             EmitDebug(request, "direction-segment selection succeeded");
 
             var projector = new FrenetProjector();
-            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
+            double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
+                : segment.LengthMeters;
+            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
                     configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
             {
                 return Failure(EmPlanningStatus.ProjectionFailed, request,
-                    "Vehicle pose could not be projected inside the selected direction segment.");
+                    "Vehicle pose could not be projected at an admissible start of the selected direction segment.");
+            }
+            if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
             }
             EmitDebug(request, "bounded ego projection succeeded");
 
@@ -78,9 +87,13 @@ public sealed class EmPlanningService : IEmPlanningService
 
             var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
                 request.Vehicle, configuration, previousSeed);
+            TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
+            var solveBudgetStopwatch = Stopwatch.StartNew();
             LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
             if (!IsSuccess(lateral.Status))
                 return Failure(lateral.Status, request, lateral.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
             EmitDebug(request, "LS optimization and validation succeeded");
 
             EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
@@ -106,8 +119,16 @@ public sealed class EmPlanningService : IEmPlanningService
                 new LongitudinalPreviousTrajectorySeedBuilder().Build(
                     request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
                     segment.SegmentIndex, segment.Direction);
+            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
+            if (remainingSolveBudget <= TimeSpan.Zero)
+            {
+                return Failure(EmPlanningStatus.SolverTimedOut, request,
+                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
+            }
+            EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
+            longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
             var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
-                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
                 request.PlanningScope, knotSchedule,
                 previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
             envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
@@ -118,6 +139,8 @@ public sealed class EmPlanningService : IEmPlanningService
             LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
             if (!IsSuccess(longitudinal.Status))
                 return Failure(longitudinal.Status, request, longitudinal.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
             EmitDebug(request, "ST optimization and validation succeeded");
 
             var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
@@ -128,6 +151,8 @@ public sealed class EmPlanningService : IEmPlanningService
                 longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
             if (assemblyStatus != EmPlanningStatus.Success)
                 return Failure(assemblyStatus, request, assemblyFailure);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
             EmitDebug(request, "trajectory assembly succeeded");
 
             EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
@@ -135,7 +160,7 @@ public sealed class EmPlanningService : IEmPlanningService
                 ? TerminalPose(lateral.Path)
                 : null;
             EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
-                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
+                request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
                 terminalPose, terminalBoundary);
             if (!publication.IsValid)
             {
@@ -144,6 +169,9 @@ public sealed class EmPlanningService : IEmPlanningService
             }
             EmitDebug(request, "world-space publication validation succeeded");
 
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
+
             EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
                 longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                 ? EmPlanningStatus.SuccessWithFallback