chore: save current workspace progress

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -17,10 +17,12 @@ internal static class EmPlanningServiceChecks
         VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
         VerifiesServicePublishesRollingApproachAndExactStopModes();
         VerifiesFullScopePublishesItsRequestScope();
+        VerifiesFullScopeAdmitsOnlyTheTrueSegmentStart();
         VerifiesRequestAndStateFailuresPublishNoTrajectory();
         VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
         VerifiesNoProgressPublishesNoTrajectory();
         VerifiesTimeoutFallbackAndCancellationSemantics();
+        VerifiesLsAndStShareOneSolveBudget();
         VerifiesPublicationFailureAndDebugIsolation();
         VerifiesPublicationGateStatusMappings();
     }
@@ -123,6 +125,32 @@ internal static class EmPlanningServiceChecks
             "full publication reaches the real segment boundary");
     }
 
+    private static void VerifiesFullScopeAdmitsOnlyTheTrueSegmentStart()
+    {
+        EmPlanningRequest uRequest = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            CreateAllForwardUReferencePath(), CreateMap(false, 12d), EmPlanningScope.FullDirectionSegment);
+        DateTimeOffset capturedAt = uRequest.RequestedAtUtc;
+        uRequest = ReplaceState(uRequest, new VehicleMotionState(
+            new Pose2D(5d, 0.4d, Math.PI), 0d, 0d, capturedAt, 8L));
+
+        EmPlanningResult laterBranch = new EmPlanningService(
+            new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(uRequest, CancellationToken.None);
+
+        VerifyFailure(laterBranch, EmPlanningStatus.ProjectionFailed,
+            "FullDirection later U branch is not an admissible segment start");
+
+        EmPlanningRequest oppositeHeading = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            planningScope: EmPlanningScope.FullDirectionSegment);
+        oppositeHeading = ReplaceState(oppositeHeading, new VehicleMotionState(
+            new Pose2D(0d, 0d, Math.PI), 0d, 0d, oppositeHeading.RequestedAtUtc, 9L));
+
+        EmPlanningResult foldedHeading = new EmPlanningService(
+            new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(oppositeHeading, CancellationToken.None);
+
+        VerifyFailure(foldedHeading, EmPlanningStatus.ProjectionFailed,
+            "FullDirection opposite heading is rejected before tangent slope conversion");
+    }
+
     private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
     {
         configuration.Scheduling.DistanceHorizonMeters = 5d;
@@ -396,6 +424,38 @@ internal static class EmPlanningServiceChecks
             VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(request,
                 cancellation.Token), EmPlanningStatus.Cancelled, "cancellation");
         }
+
+        using (var cancellation = new CancellationTokenSource())
+        {
+            EmPlanningRequest publicationRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
+            publicationRequest.Configuration.Solver.NativeVerbose = true;
+            var service = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success),
+                new CancellingPublicationDebugSink(cancellation));
+
+            EmPlanningResult cancelledBeforePublication = service.Plan(publicationRequest, cancellation.Token);
+
+            VerifyFailure(cancelledBeforePublication, EmPlanningStatus.Cancelled,
+                "cancellation immediately before service publication");
+        }
+    }
+
+    private static void VerifiesLsAndStShareOneSolveBudget()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        request.Configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        request.Configuration.Solver.MaximumOuterIterations = 1;
+        var solver = new ScriptedPipelineSolver(PipelineSolverMode.Success,
+            lateralSolveDelay: TimeSpan.FromMilliseconds(200d));
+
+        EmPlanningResult result = new EmPlanningService(solver).Plan(request, CancellationToken.None);
+
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            "shared LS/ST budget test publishes a validated trajectory");
+        Verification.True(solver.LateralSolveBudgets.Count > 0 && solver.LongitudinalSolveBudgets.Count > 0,
+            "shared LS/ST budget test reached both optimizers");
+        Verification.True(solver.LongitudinalSolveBudgets[0] <
+                solver.LateralSolveBudgets[0] - TimeSpan.FromMilliseconds(100d),
+            "ST receives only the LS/ST budget remaining after the delayed LS solve");
     }
 
     private static void VerifiesNoProgressPublishesNoTrajectory()
@@ -616,6 +676,28 @@ internal static class EmPlanningServiceChecks
             new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
     }
 
+    private static PathSmoothingResult CreateAllForwardUReferencePath()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
+                0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(10d, 0d, 0d, 0d, 10d, TravelDirection.Forward,
+                0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(10d, 0.4d, Math.PI, Math.PI, 10.4d, TravelDirection.Forward,
+                0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(0d, 0.4d, Math.PI, Math.PI, 20.4d, TravelDirection.Forward,
+                0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+        };
+        var segments = new List<SmoothedPathSegment>
+        {
+            new SmoothedPathSegment(0, TravelDirection.Forward, 0, points.Count - 1, false, false),
+        };
+        var metrics = new PathQualityMetrics(true, 20.4d, 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>());
+    }
+
     private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d)
     {
         IMapObstacleSource[] sources = blockStart
@@ -651,16 +733,20 @@ internal static class EmPlanningServiceChecks
         private readonly PipelineSolverMode mode;
         private readonly PlanningGridMap? mapToCorrupt;
         private readonly IReadOnlyList<double>? strictFullPrimal;
+        private readonly TimeSpan lateralSolveDelay;
         private int longitudinalCallCount;
 
         public QuadraticProgram? LastLongitudinalProblem { get; private set; }
+        public List<TimeSpan> LateralSolveBudgets { get; } = new();
+        public List<TimeSpan> LongitudinalSolveBudgets { get; } = new();
 
         public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null,
-            IReadOnlyList<double>? strictFullPrimal = null)
+            IReadOnlyList<double>? strictFullPrimal = null, TimeSpan? lateralSolveDelay = null)
         {
             this.mode = mode;
             this.mapToCorrupt = mapToCorrupt;
             this.strictFullPrimal = strictFullPrimal;
+            this.lateralSolveDelay = lateralSolveDelay.GetValueOrDefault();
         }
 
         public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
@@ -671,12 +757,16 @@ internal static class EmPlanningServiceChecks
                 return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
             if (!longitudinal)
             {
+                LateralSolveBudgets.Add(settings.TimeLimit);
+                if (lateralSolveDelay > TimeSpan.Zero)
+                    Thread.Sleep(lateralSolveDelay);
                 if (mode == PipelineSolverMode.LateralInfeasible)
                     return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
                 if (mode == PipelineSolverMode.TimeoutWithoutFallback)
                     return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
                 return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
             }
+            LongitudinalSolveBudgets.Add(settings.TimeLimit);
             LastLongitudinalProblem = problem;
             if (mode == PipelineSolverMode.LongitudinalInfeasible)
                 return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
@@ -928,7 +1018,10 @@ internal static class EmPlanningServiceChecks
             if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0)
                 return false;
             int knotCount = (problem.VariableCount + 1) / 4;
-            return problem.ConstraintCount >= 8 * knotCount - 2;
+            var layout = new LongitudinalVariableLayout(knotCount);
+            return TryReadFixedVariable(problem, layout.S(0), out _) &&
+                TryReadFixedVariable(problem, layout.U(0), out _) &&
+                TryReadFixedVariable(problem, layout.A(0), out _);
         }
 
         private static double ReadFixedVariable(QuadraticProgram problem, int variable)
@@ -980,4 +1073,20 @@ internal static class EmPlanningServiceChecks
             throw new InvalidOperationException("debug sink failure");
         }
     }
+
+    private sealed class CancellingPublicationDebugSink : IEmPlannerDebugSink
+    {
+        private readonly CancellationTokenSource cancellation;
+
+        public CancellingPublicationDebugSink(CancellationTokenSource cancellation)
+        {
+            this.cancellation = cancellation ?? throw new ArgumentNullException(nameof(cancellation));
+        }
+
+        public void Write(string message)
+        {
+            if (string.Equals(message, "world-space publication validation succeeded", StringComparison.Ordinal))
+                cancellation.Cancel();
+        }
+    }
 }