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();
+        }
+    }
 }

ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs

@@ -60,3 +60,29 @@ internal sealed class FakeQpSolver : IQpSolver
         return _results.Dequeue();
     }
 }
+
+internal sealed class CancellingQpSolver : IQpSolver
+{
+    private readonly IQpSolver inner;
+    private readonly CancellationTokenSource cancellation;
+    private readonly int cancelAfterSolveCount;
+    private int solveCount;
+
+    public CancellingQpSolver(IQpSolver inner, CancellationTokenSource cancellation, int cancelAfterSolveCount = 1)
+    {
+        this.inner = inner ?? throw new ArgumentNullException(nameof(inner));
+        this.cancellation = cancellation ?? throw new ArgumentNullException(nameof(cancellation));
+        if (cancelAfterSolveCount <= 0) throw new ArgumentOutOfRangeException(nameof(cancelAfterSolveCount));
+        this.cancelAfterSolveCount = cancelAfterSolveCount;
+    }
+
+    public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
+        CancellationToken cancellationToken)
+    {
+        QpSolveResult result = inner.Solve(problem, settings, warmStart, cancellationToken);
+        solveCount++;
+        if (solveCount == cancelAfterSolveCount)
+            cancellation.Cancel();
+        return result;
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs

@@ -16,6 +16,8 @@ internal static class LateralIntegrationChecks
     {
         VerifiesValidatedCandidateSurvivesLaterTimeout();
         VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
+        VerifiesCancellationAfterStrictCandidateNeverPublishesFallback();
+        VerifiesRejectedCandidateAdvancesTheNextLateralLinearization();
         VerifiesTrustRegionWarmStartAndOuterIterationLimit();
         VerifiesCancellationAndTimeoutWithoutCandidate();
         VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
@@ -150,6 +152,47 @@ internal static class LateralIntegrationChecks
             "SolvedInaccurate still requires full independent lateral validation");
     }
 
+    private static void VerifiesCancellationAfterStrictCandidateNeverPublishesFallback()
+    {
+        LateralPlanningInput input = CreateInput();
+        double[] valid = CreatePrimal(input, 0.02d);
+        using var cancellation = new CancellationTokenSource();
+        var solver = new CancellingQpSolver(
+            new FakeQpSolver(Result(QpSolveStatus.Solved, valid, 10d)), cancellation);
+
+        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, cancellation.Token);
+
+        Verification.Equal(EmPlanningStatus.Cancelled, result.Status,
+            "cancellation after a strict LS candidate is never fallback success");
+        Verification.True(result.Path == null, "cancelled LS result exposes no path");
+    }
+
+    private static void VerifiesRejectedCandidateAdvancesTheNextLateralLinearization()
+    {
+        LateralPlanningInput input = CreateInput(0.10d);
+        double[] strict = CreatePrimal(input, 0.01d);
+        double[] rejected = CreatePrimal(input, 0.02d);
+        var solver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, strict, 10d),
+            Result(QpSolveStatus.Solved, rejected, 9d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
+        });
+
+        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
+
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+            "later timeout preserves the earlier strict candidate");
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        Verification.NearlyEqual(0.02d, solver.WarmStarts[2][layout.L(1)],
+            "the next LS QP warm-starts from the rejected but parseable candidate");
+        FindSingleVariableBounds(solver.Problems[2], layout.L(1), out double lower, out double upper);
+        Verification.NearlyEqual(-0.03d, lower,
+            "the next LS trust region is centered on the rejected candidate");
+        Verification.NearlyEqual(0.07d, upper,
+            "the next LS trust region is centered on the rejected candidate");
+    }
+
     private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
     {
         LateralPlanningInput input = CreateInput();
@@ -178,7 +221,10 @@ internal static class LateralIntegrationChecks
         var limitSolver = new FakeQpSolver(limitResults);
         LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
         Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
-        Verification.Equal(EmPlanningStatus.Success, limited.Status, "last feasible candidate succeeds at outer iteration limit");
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, limited.Status,
+            "last strict candidate is reported as fallback at outer iteration limit");
+        Verification.True(limited.FailureReason.Length > 0,
+            "outer iteration fallback preserves a non-empty diagnostic");
     }
 
     private static void VerifiesCancellationAndTimeoutWithoutCandidate()
@@ -319,7 +365,7 @@ internal static class LateralIntegrationChecks
             Verification.True(Math.Abs(leftValues[index] - rightValues[index]) <= 1e-10d, name + " value " + index);
     }
 
-    private static LateralPlanningInput CreateInput()
+    private static LateralPlanningInput CreateInput(double maximumVehicleCurvature = 1d)
     {
         var points = new List<SmoothedPathPoint>
         {
@@ -341,7 +387,7 @@ internal static class LateralIntegrationChecks
             LengthMeters = 0.1d,
             WidthMeters = 0.1d,
             SafetyMarginMeters = 0d,
-            MaximumCurvaturePerMeter = 1d,
+            MaximumCurvaturePerMeter = maximumVehicleCurvature,
         };
         return new LateralPlanningInput(segment, corridor,
             new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),

ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs

@@ -18,6 +18,7 @@ internal static class LateralModelChecks
         VerifiesPlanningInputBoundariesAndDefensiveCopies();
         VerifiesLateralResultPublicationContract();
         VerifiesNormalizedObjectiveAndHardConstraints();
+        VerifiesVehicleCurvatureIsAHardQpConstraint();
         VerifiesAllNamedCostScales();
         VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
         VerifiesFakeSolverCapturesTheNeutralQpBoundary();
@@ -245,6 +246,29 @@ internal static class LateralModelChecks
             "Frenet denominator is intersected as a finite hard lateral bound");
     }
 
+    private static void VerifiesVehicleCurvatureIsAHardQpConstraint()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        const double maximumVehicleCurvature = 0.25d;
+        LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
+            Array.Empty<double>(), maximumVehicleCurvature: maximumVehicleCurvature);
+        LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
+            new[] { 0d, 0d });
+
+        Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
+                out string failureReason), "curvature-constrained QP builds: " + failureReason);
+
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        Verification.Equal(8 * layout.StationCount - 2, problem.ConstraintCount,
+            "each lateral station adds one curvature hard-constraint row");
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            Verification.True(HasBound(problem, new Dictionary<int, double> { { layout.DDL(station), 1d } },
+                    -maximumVehicleCurvature, maximumVehicleCurvature),
+                "straight-reference curvature is a hard DDL bound at station " + station);
+        }
+    }
+
     private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
     {
         EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
@@ -550,6 +574,21 @@ internal static class LateralModelChecks
         return false;
     }
 
+    private static bool HasBound(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
+        double lower, double upper)
+    {
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            if (RowMatches(problem.ConstraintMatrix, row, expected) &&
+                Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
+                Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+
     private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
     {
         var actual = new Dictionary<int, double>();

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -554,6 +554,18 @@ internal static class LongitudinalIntegrationChecks
             Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled ST does not call the QP solver");
         }
 
+        using (var cancellation = new CancellationTokenSource())
+        {
+            var solver = new CancellingQpSolver(
+                new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d)), cancellation);
+            LongitudinalPlanningResult cancelledAfterStrictCandidate = new SequentialLongitudinalOptimizer(solver).Optimize(
+                input, cancellation.Token);
+            Verification.Equal(EmPlanningStatus.Cancelled, cancelledAfterStrictCandidate.Status,
+                "cancellation after a strict ST candidate is never fallback success");
+            Verification.True(cancelledAfterStrictCandidate.Candidate == null,
+                "cancelled ST result exposes no candidate");
+        }
+
         var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
         LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
             CancellationToken.None);

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -12,6 +12,7 @@ internal static class TrajectoryChecks
     public static void Run()
     {
         VerifiesForwardFieldsExactTerminalAndHold();
+        VerifiesSegmentLocalReferenceStationIsPublished();
         VerifiesReverseTravelVelocityAndUnwrappedYaw();
         VerifiesPublishedListsAreImmutable();
         VerifiesRollingTrajectoryHasNoSyntheticStopTail();
@@ -49,6 +50,30 @@ internal static class TrajectoryChecks
             "reverse yaw interpolation unwraps across the pi boundary");
     }
 
+    private static void VerifiesSegmentLocalReferenceStationIsPublished()
+    {
+        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal));
+
+        EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
+        Verification.NearlyEqual(1d, terminal.SegmentLocalS,
+            "published SegmentLocalS retains the lateral path reference station");
+        Verification.NearlyEqual(0.12d, terminal.PathS,
+            "published PathS remains the longitudinal actual path distance");
+
+        ValidationContext context = CreateValidationContext();
+        EmTrajectory validationTrajectory = CreateValidationTrajectory(TravelDirection.Forward);
+        EmTrajectoryValidationResult accepted = new EmTrajectoryValidator().Validate(validationTrajectory, context.EmptyMap,
+            context.Vehicle, context.Configuration, 2, 1d, 0.0055d, EmBoundaryType.Goal);
+        Verification.True(accepted.IsValid,
+            "publication validates reference and actual path bounds independently: " + accepted.Message);
+        EmTrajectoryValidationResult segmentExceeded = new EmTrajectoryValidator().Validate(validationTrajectory,
+            context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.5d, 0.0055d, EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, segmentExceeded.Failure,
+            "segment-local reference bound is checked independently of PathS");
+    }
+
     private static void VerifiesPublishedListsAreImmutable()
     {
         EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -39,7 +39,8 @@ internal static class TrajectoryObservationChecks
         VerifiesObserverTicksWhilePlanningIsDelayed();
         VerifiesFullDirectionSegmentPlansOncePerActiveSegment();
         VerifiesFullDirectionRequestCarriesValidatedScope();
-        VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition();
+        VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation();
+        VerifiesDeterministicSingleForwardObservationSimulation();
         VerifiesFailedFullPlanDoesNotAutoRetry();
         VerifiesStaticSnapshotExportsPlanningScope();
         VerifiesSessionLayerCleanupDecisions();
@@ -114,7 +115,7 @@ internal static class TrajectoryObservationChecks
 
         Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
             "observation MovementTest uses required Chinese display name");
-        Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段",
+        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段",
             TrajectoryObservationRuntimeState.GearSwitchWaitingNotice,
             "observation runtime uses required gear-switch notice");
         Verification.True(source.Contains("Console.WriteLine(\"[TrajectoryObserver] \" + text);"),
@@ -180,6 +181,10 @@ internal static class TrajectoryObservationChecks
         Verification.True(normalizedSource.Contains(
                 "VehicleMotionState state = ReadVehicleState();\n            DateTimeOffset now = state.CapturedAtUtc;"),
             "observer host uses the fresh state snapshot time for each observation tick");
+        Verification.True(source.Contains("effectiveConfiguration.Frenet.MaximumProjectionDistanceMeters"),
+            "native observation charts use the EM Frenet projection tolerance rather than map padding");
+        Verification.True(source.Contains("tick.SegmentState.Phase == TrajectoryObservationSegmentPhase.Completed"),
+            "observer session terminates after the final direction segment completes");
         Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
             "observer settings use the ST timestamp-spacing default");
 
@@ -524,7 +529,7 @@ internal static class TrajectoryObservationChecks
             "full scope blocks coordinator-cadence restart after the first plan");
     }
 
-    private static void VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition()
+    private static void VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation()
     {
         DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 2, 0, 0, TimeSpan.Zero);
         var settings = new TrajectoryObservationSettings
@@ -550,28 +555,90 @@ internal static class TrajectoryObservationChecks
         var stoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L);
         controller.StartCycle(t0, stoppedState, CancellationToken.None).GetAwaiter().GetResult();
 
-        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)),
-            "no replan is armed before a confirmed segment transition");
         controller.TryAdvanceSegment(t0, stoppedState);
-        controller.TryAdvanceSegment(t0.AddSeconds(0.21d),
-            new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L));
-        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)),
-            "stop hold and direction waiting do not reset the one-shot flag");
+        DateTimeOffset stoppedAt = t0.AddSeconds(0.21d);
+        var heldStoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, stoppedAt, 2L);
+        controller.TryAdvanceSegment(stoppedAt, heldStoppedState);
+        Verification.True(controller.ShouldStartCycle(stoppedAt),
+            "the next full direction segment is planned after the real stop hold, before direction evidence");
+
+        PlanningCycleResult pendingCycle = controller.StartCycle(stoppedAt, heldStoppedState, CancellationToken.None)
+            .GetAwaiter().GetResult();
+        Verification.True(pendingCycle.Published, "the stopped-state pending segment plan is independently publishable");
+        Verification.Equal(2, planningService.Requests.Count,
+            "the next segment is planned exactly once from the stopped state");
+        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
+            "the stopped-state pending request targets segment N+1");
+        Verification.Equal(heldStoppedState.SequenceId, planningService.Requests[1].VehicleState.SequenceId,
+            "the pending segment request uses the real stopped vehicle state");
+        Verification.True(planningService.Requests[1].PreviousTrajectory == null,
+            "a pending direction segment never reuses the old direction trajectory as an EM seed");
+        Verification.Equal(0, controller.ActiveSegment.SegmentIndex,
+            "planning N+1 does not replace the old segment before direction confirmation");
+        Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PublishedTrajectory),
+            "the old segment trajectory remains active while N+1 waits for confirmation");
 
         var reverseState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null,
             t0.AddSeconds(0.22d), 3L);
         Verification.True(controller.TryAdvanceSegment(t0.AddSeconds(0.22d), reverseState),
             "confirmed N to N+1 transition succeeds");
-        Verification.True(controller.ShouldStartCycle(t0.AddSeconds(0.23d)),
-            "one-shot planning is rearmed only after the confirmed transition");
-
-        controller.StartCycle(t0.AddSeconds(0.23d), reverseState, CancellationToken.None).GetAwaiter().GetResult();
+        Verification.Equal(1, controller.ActiveSegment.SegmentIndex,
+            "the pending plan becomes active only after direction confirmation");
+        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(0.23d)),
+            "confirmed activation does not trigger a duplicate full-segment plan");
         Verification.Equal(2, planningService.Requests.Count,
-            "the next active direction segment receives exactly one new plan");
-        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
-            "the new full-direction plan targets segment N+1");
-        Verification.Equal(EmPlanningScope.FullDirectionSegment, planningService.Requests[1].PlanningScope,
-            "the new full-direction plan keeps the full scope");
+            "the pending plan is reused at activation instead of planning N+1 twice");
+        Verification.Equal(reverseState.CapturedAtUtc, controller.PublishedTrajectory.Metadata.EffectiveAtUtc,
+            "pending trajectory effective time is rebased to its real activation time");
+    }
+
+    private static void VerifiesDeterministicSingleForwardObservationSimulation()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 8, 0, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings
+        {
+            PlanningScope = EmPlanningScope.FullDirectionSegment,
+        };
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
+            .Bootstrap(job, CancellationToken.None);
+        Verification.True(baseBootstrap.Succeeded, "deterministic single-forward simulation bootstrap succeeds");
+        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
+            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, new[]
+            {
+                CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false,
+                    EmBoundaryType.None, EmBoundaryType.Goal),
+            });
+        EmTrajectory trajectory = CreateSingleForwardSimulationTrajectory(t0);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedTrajectoryPlanningService(trajectory), "single-forward-simulation");
+        var loop = new TrajectoryObservationLoop(controller);
+
+        double x = 0d;
+        double heading = 0d;
+        var initialState = new VehicleMotionState(new Pose2D(x, 0d, heading), 0d, null, t0, 1L);
+        controller.StartCycle(t0, initialState, CancellationToken.None).GetAwaiter().GetResult();
+        TrajectoryControlCommand command = controller.Observe(t0, initialState).Command;
+        TrajectoryObservationLoopTick? finalTick = null;
+        const double deltaSeconds = 0.10d;
+        for (int step = 1; step <= 10; step++)
+        {
+            x += command.SignedLongitudinalVelocity * Math.Cos(heading) * deltaSeconds;
+            heading += command.YawRate * deltaSeconds;
+            DateTimeOffset now = t0.AddSeconds(step * deltaSeconds);
+            var state = new VehicleMotionState(new Pose2D(x, 0d, heading), command.SignedLongitudinalVelocity,
+                null, now, step + 1L);
+            finalTick = loop.Tick(now, state, CancellationToken.None);
+            command = finalTick.Observation.Command;
+        }
+
+        Verification.NearlyEqual(1d, x, "simulated forward vehicle reaches the real Goal pose from executor commands");
+        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, finalTick!.SegmentState.Phase,
+            "simulated forward Goal completes the observation lifecycle");
+        Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity,
+            "simulated forward Goal receives the terminal zero-speed command");
     }
 
     private static void VerifiesFailedFullPlanDoesNotAutoRetry()
@@ -666,11 +733,24 @@ internal static class TrajectoryObservationChecks
             effectiveAt.AddSeconds(1d), trajectory);
         Verification.True(atFinal.WaitingAtGearSwitch,
             "observer enters gear-switch wait state at final time");
-        Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段", atFinal.WorldNotice,
+        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段", atFinal.WorldNotice,
             "observer exposes the exact gear-switch state to the world painter");
         Verification.Equal(0, trajectory.Metadata.SegmentIndex,
             "observer gear-switch wait state remains on segment zero");
 
+        var tracker = new TrajectoryObservationSegmentTracker(CreateDirectionalSegments(),
+            new TrajectoryObservationSettings(), 0.01d);
+        TrajectoryObservationRuntimeState planningState = TrajectoryObservationRuntimeState.Create(tracker.State);
+        Verification.True(!planningState.WaitingAtGearSwitch,
+            "tracker-planning state does not show a gear-switch wait notice merely because planned time elapsed");
+        tracker.Update(effectiveAt.AddSeconds(1d), new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
+                effectiveAt.AddSeconds(1d), 1L),
+            CreateSegmentTrajectory(effectiveAt, "runtime-forward", 0, TravelDirection.Forward,
+                EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d));
+        TrajectoryObservationRuntimeState stoppedState = TrajectoryObservationRuntimeState.Create(tracker.State);
+        Verification.True(stoppedState.WaitingAtGearSwitch,
+            "actual tracker stop state shows the gear-switch wait notice");
+
         string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
             "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
         string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
@@ -780,6 +860,7 @@ internal static class TrajectoryObservationChecks
         DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 4, 0, 0, TimeSpan.Zero);
         var settings = new TrajectoryObservationSettings
         {
+            PlanningScope = EmPlanningScope.RollingHorizon,
             DirectionConfirmationSamples = 1,
         };
         CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
@@ -1244,6 +1325,20 @@ internal static class TrajectoryObservationChecks
         });
     }
 
+    private static EmTrajectory CreateSingleForwardSimulationTrajectory(DateTimeOffset effectiveAt)
+    {
+        var metadata = new EmTrajectoryMetadata("single-forward-simulation", effectiveAt, effectiveAt, 1L,
+            "single-forward-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal,
+            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 0d, 1d, 0d, 0d, 0, 0d, 0d,
+                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(1d, 0d, 0d, 0d, 1d, 0d, 0, 1d, 1d,
+                TravelDirection.Forward, EmBoundaryType.Goal, 0d, 0d),
+        });
+    }
+
     private sealed class FixedTrajectoryPlanningService : IEmPlanningService
     {
         private readonly EmTrajectory trajectory;

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs

@@ -17,6 +17,7 @@ internal static class TrajectoryObservationSegmentChecks
         RejectsNonTerminalOrMismatchedGearTrajectory();
         ResetsConfirmationForInvalidDirectionEvidence();
         CompletesOneSegmentWithoutIndexingPastTheEnd();
+        CompletesSingleForwardGoalTrajectory();
     }
 
     private static void RejectsHardcodedActiveSegmentIndex()
@@ -112,6 +113,22 @@ internal static class TrajectoryObservationSegmentChecks
         Verification.Equal(0, tracker.State.ActiveSegmentIndex, "completed tracker retains final segment index");
     }
 
+    private static void CompletesSingleForwardGoalTrajectory()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 3, 30, 0, TimeSpan.Zero);
+        var tracker = new TrajectoryObservationSegmentTracker(new[]
+        {
+            CreateSegment(0, TravelDirection.Forward, 0d, 1d, false, 0d, EmBoundaryType.None, EmBoundaryType.Goal),
+        }, CreateSettings(), 0.01d);
+
+        TrajectoryObservationSegmentUpdate completed = tracker.Update(t0, StateAtSwitch(0d, t0, 1L),
+            GoalTerminal(t0));
+
+        Verification.True(completed.Completed, "single forward Goal trajectory reports completion");
+        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, tracker.State.Phase,
+            "single forward Goal reaches completed without requiring a gear-switch terminal");
+    }
+
     private static void AssertConfirmationReset(DateTimeOffset t0, VehicleMotionState invalidState,
         EmTrajectory invalidTrajectory, string name)
     {
@@ -196,4 +213,16 @@ internal static class TrajectoryObservationSegmentChecks
                 direction, EmBoundaryType.GearSwitchApproach, 0d, 0d),
         });
     }
+
+    private static EmTrajectory GoalTerminal(DateTimeOffset effectiveAtUtc)
+    {
+        var metadata = new EmTrajectoryMetadata("goal-terminal-" + effectiveAtUtc.Ticks, effectiveAtUtc,
+            effectiveAtUtc, 1L, "segment-check", 1L, string.Empty, 0, TravelDirection.Forward,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(1d, 0d, 0d, 0d, 0d, 0d, 0, 1d, 1d,
+                TravelDirection.Forward, EmBoundaryType.Goal, 0d, 0d),
+        });
+    }
 }