using System; using System.Collections.Generic; using System.IO; using System.Text; using System.Threading; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade; using MultiWheelC.TrajectoryPlanning.EMPlanner; using MultiWheelC.TrajectoryPlanning.Mapping; using MultiWheelC.TrajectoryPlanning.PathSmoothing; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade; using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; namespace EMPlannerVerificationHost; internal static class TrajectoryObservationChecks { public static void Run() { TrajectoryObservationSettingsChecks.Run(); TrajectoryObservationSegmentChecks.Run(); VerifiesObservationSourceHasNoActuatorCalls(); VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesOperatorDocumentationUsesExactUiEntry(); VerifiesMovementTestVehicleInputsAndSettingsSnapshot(); VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration(); RejectsInvalidObservationSettings(); VerifiesStartGoalBoundsUseOnlyConfiguredPadding(); VerifiesValidNonEmptyObstacleSource(); RejectsObstacleOutsideConfiguredBounds(); VerifiesObserverTicksWhilePlanningIsDelayed(); VerifiesSessionLayerCleanupDecisions(); VerifiesGearSwitchWaitStateForWorldPresentation(); VerifiesLsAndStUsePublishedTrajectoryData(); VerifiesPresentationTextDescribesObservationWithoutSendingCommand(); VerifiesPlanningDiagnosticsKeepRawFailureReason(); VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary(); VerifiesEmptyChartsReceivePersistentPlanningDiagnostic(); VerifiesLsPresentationUsesPathSOnHorizontalAxis(); VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot(); FreezesBootstrapVehicleForRollingRequests(); VerifiesEmptyMapBootstrapProducesPublishableReference(); } private static void VerifiesObservationSourceHasNoActuatorCalls() { string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest"); string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs"); Verification.True(File.Exists(movementTestSource), "observation MovementTest source exists"); string[] forbiddenTokens = { ".SendXYThSpeed(", ".SendMotion(", ".SendTh(", ".AccumulateSpeed(", ".ComputeWheelsGeometrically(", ".DriveStop(", ".PredefinedDriveStop(" }; string[] runtimeSources = Directory.GetFiles(sourceDirectory, "*.cs", SearchOption.TopDirectoryOnly); for (int sourceIndex = 0; sourceIndex < runtimeSources.Length; sourceIndex++) { string source = File.ReadAllText(runtimeSources[sourceIndex]); for (int tokenIndex = 0; tokenIndex < forbiddenTokens.Length; tokenIndex++) { Verification.True(source.IndexOf(forbiddenTokens[tokenIndex], StringComparison.Ordinal) < 0, "observation runtime source excludes actuator token " + forbiddenTokens[tokenIndex]); } } } private static void VerifiesObservationSourceUsesRequiredOperatorText() { string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs"); string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource)); Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"), "observation MovementTest uses required Chinese display name"); Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段", TrajectoryObservationRuntimeState.GearSwitchWaitingNotice, "observation runtime uses required gear-switch notice"); Verification.True(source.Contains("Console.WriteLine(\"[TrajectoryObserver] \" + text);"), "observer status is mirrored to the host terminal"); } private static void VerifiesOperatorDocumentationUsesExactUiEntry() { string sourceDirectory = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest"); string movementTestSource = Path.Combine(sourceDirectory, "MovementTest.TrajectoryObservationTest.cs"); string readmePath = Path.Combine(sourceDirectory, "README.md"); string movementTestText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource)); string readmeText = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(readmePath)); const string uiEntry = "EM轨迹规划观察闭环测试"; Verification.True(movementTestText.Contains("[MovementTest(name = \"" + uiEntry + "\")]"), "observation MovementTest has the exact UI entry"); Verification.True(readmeText.Contains(uiEntry), "observation README has the exact UI entry"); } private static void VerifiesMovementTestVehicleInputsAndSettingsSnapshot() { string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs"); string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource)); Verification.True(source.Contains("public double VehicleLengthMeters = 0.80d;"), "observer MovementTest exposes vehicle length with Task-1 default"); Verification.True(source.Contains("public double VehicleWidthMeters = 0.60d;"), "observer MovementTest exposes vehicle width with Task-1 default"); Verification.True(source.Contains("public double SafetyMarginMeters = 0.05d;"), "observer MovementTest exposes safety margin with Task-1 default"); Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"), "observer MovementTest exposes maximum curvature with Task-1 default"); Verification.True(source.Contains("public double SolverTimeoutSeconds ="), "observer MovementTest exposes a configurable solver timeout"); Verification.True(source.Contains("public int MaximumOsqpIterations ="), "observer MovementTest exposes a configurable OSQP iteration limit"); Verification.True(source.Contains("public double TimeHorizonSeconds ="), "observer MovementTest exposes a configurable ST time horizon"); Verification.True(source.Contains("public double OutputTimeStepSeconds ="), "observer MovementTest exposes a configurable ST timestamp spacing"); Verification.True(source.Contains("不等于观察循环周期"), "observer MovementTest documents the distinction between output timestamps and observer ticks"); Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"), "observer MovementTest copies vehicle length into settings"); Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"), "observer MovementTest copies vehicle width into settings"); Verification.True(source.Contains("SafetyMarginMeters = SafetyMarginMeters,"), "observer MovementTest copies safety margin into settings"); Verification.True(source.Contains("MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,"), "observer MovementTest copies maximum curvature into settings"); Verification.True(source.Contains("SolverTimeoutSeconds = SolverTimeoutSeconds,"), "observer MovementTest copies solver timeout into settings"); Verification.True(source.Contains("MaximumOsqpIterations = MaximumOsqpIterations,"), "observer MovementTest copies OSQP iteration limit into settings"); Verification.True(source.Contains("TimeHorizonSeconds = TimeHorizonSeconds,"), "observer MovementTest copies ST time horizon into settings"); Verification.True(source.Contains("OutputTimeStepSeconds = OutputTimeStepSeconds,"), "observer MovementTest copies ST timestamp spacing into settings"); string normalizedSource = source.Replace("\r\n", "\n"); 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.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds, "observer settings use the ST timestamp-spacing default"); var configured = new TrajectoryObservationSettings { VehicleLengthMeters = 1.10d, VehicleWidthMeters = 0.70d, SafetyMarginMeters = 0.08d, MaximumCurvaturePerMeter = 0.55d, SolverTimeoutSeconds = 0.42d, MaximumOsqpIterations = 9000, TimeHorizonSeconds = 4d, OutputTimeStepSeconds = 0.20d, }; TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot(); configured.VehicleLengthMeters = 9.10d; configured.VehicleWidthMeters = 9.20d; configured.SafetyMarginMeters = 9.30d; configured.MaximumCurvaturePerMeter = 9.40d; configured.SolverTimeoutSeconds = 9.50d; configured.MaximumOsqpIterations = 9500; configured.TimeHorizonSeconds = 5d; configured.OutputTimeStepSeconds = 0.25d; Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters, "observer settings snapshot freezes vehicle length"); Verification.NearlyEqual(0.70d, snapshot.VehicleWidthMeters, "observer settings snapshot freezes vehicle width"); Verification.NearlyEqual(0.08d, snapshot.SafetyMarginMeters, "observer settings snapshot freezes safety margin"); Verification.NearlyEqual(0.55d, snapshot.MaximumCurvaturePerMeter, "observer settings snapshot freezes maximum curvature"); Verification.NearlyEqual(0.42d, snapshot.SolverTimeoutSeconds, "observer settings snapshot freezes solver timeout"); Verification.Equal(9000, snapshot.MaximumOsqpIterations, "observer settings snapshot freezes OSQP iteration limit"); Verification.NearlyEqual(4d, snapshot.TimeHorizonSeconds, "observer settings snapshot freezes ST time horizon"); Verification.NearlyEqual(0.20d, snapshot.OutputTimeStepSeconds, "observer settings snapshot freezes ST timestamp spacing"); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot, Array.Empty(), 0L); AssertVehicleSnapshot(job.Vehicle, 1.10d, 0.70d, 0.08d, 0.55d, null, "observer configured bootstrap vehicle"); } private static void VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings { ReplanPeriodSeconds = 0.25d, SolverTimeoutSeconds = 1.25d, MaximumOsqpIterations = 54321, TimeHorizonSeconds = 3.5d, OutputTimeStepSeconds = 0.10d, }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer configuration-diagnostic bootstrap succeeds"); var controller = new TrajectoryObservationController(bootstrap, settings, new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic"); string text = controller.CreateConfigurationDiagnostic().Text; foreach (string expected in new[] { "planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s", "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321", "solverTimeout=1.25s", "replanPeriod=0.25s", "maximumJerkLimitedStopDistance=", "maximumJerkLimitedStopDuration=", "requiredDistanceHorizon=", }) { Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected); } string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs"); string runnerSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource)); int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal); Verification.True(firstCall >= 0, "runner prints configuration diagnostic"); Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1, StringComparison.Ordinal), "runner prints configuration diagnostic once"); } private static void RejectsInvalidObservationSettings() { AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding"); AssertInvalidSetting(settings => settings.MapResolutionMillimeters = float.NaN, "map resolution"); AssertInvalidSetting(settings => settings.ReplanPeriodSeconds = 0d, "replan period"); AssertInvalidSetting(settings => settings.ObserverPeriodSeconds = double.PositiveInfinity, "observer period"); AssertInvalidSetting(settings => settings.VehicleLengthMeters = 0d, "vehicle length"); AssertInvalidSetting(settings => settings.VehicleWidthMeters = double.NaN, "vehicle width"); AssertInvalidSetting(settings => settings.SafetyMarginMeters = 0d, "safety margin"); AssertInvalidSetting(settings => settings.MaximumCurvaturePerMeter = double.PositiveInfinity, "maximum curvature"); AssertInvalidSetting(settings => settings.SolverTimeoutSeconds = 0d, "solver timeout"); AssertInvalidSetting(settings => settings.MaximumOsqpIterations = 0, "OSQP iteration limit"); AssertInvalidSetting(settings => settings.MaximumOsqpIterations = -1, "negative OSQP iteration limit"); AssertInvalidSetting(settings => settings.TimeHorizonSeconds = 0d, "ST time horizon"); AssertInvalidSetting(settings => settings.OutputTimeStepSeconds = 0d, "ST timestamp spacing"); AssertInvalidSetting(settings => { settings.TimeHorizonSeconds = 1d; settings.OutputTimeStepSeconds = 1.01d; }, "ST timestamp spacing larger than the horizon"); } private static void AssertInvalidSetting(Action mutate, string name) { var settings = new TrajectoryObservationSettings(); mutate(settings); Verification.True(Throws(settings.Validate), "observer rejects invalid " + name); } private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding() { var settings = new TrajectoryObservationSettings { MapPaddingMeters = 2d, MapResolutionMillimeters = 50f, }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings, Array.Empty(), 17L); Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min"); Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max"); Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min"); Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max"); Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution"); } private static void VerifiesValidNonEmptyObstacleSource() { CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(), new[] { TrajectoryObservationObstacle.Circle(500d, 500d, 100d) }, 23L); Verification.True(!job.MapRequest.AllowExplicitEmptyMap, "observer non-empty obstacle map is not explicitly empty"); Verification.Equal(1, job.MapRequest.ObstacleSources.Count, "observer non-empty map has exactly one obstacle source"); IMapObstacleSource source = job.MapRequest.ObstacleSources[0]; Verification.Equal("trajectory-observer-manual", source.SourceId, "observer manual obstacle source ID"); Verification.Equal(23L, source.SourceVersion, "observer manual obstacle source version"); Verification.True(source.IsRequired, "observer manual obstacle source is required"); ObstacleProjectionResult projection = source.ProjectToWorld(); Verification.Equal(ObstacleSourceStatus.Applied, projection.Status, "observer valid manual obstacle source applies"); Verification.Equal(1, projection.Obstacles.Count, "observer valid manual obstacle source preserves geometry"); } private static void RejectsObstacleOutsideConfiguredBounds() { Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(), new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)), "observer obstacle outside configured bounds"); } private static void VerifiesObserverTicksWhilePlanningIsDelayed() { DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 4, 3, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings(); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer delayed-planner bootstrap succeeds"); EmTrajectory published = CreatePublishedTrajectory(startedAt); var planningService = new DelayedTrajectoryPlanningService(published, 2); try { var controller = new TrajectoryObservationController( bootstrap, settings, planningService, "cadence-check"); var loop = new TrajectoryObservationLoop(controller); var firstState = new VehicleMotionState( new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 10L); TrajectoryObservationLoopTick firstTick = loop.Tick(startedAt, firstState, CancellationToken.None); Verification.True(firstTick.PlanningStarted, "observer first tick reports a planning-cycle start"); Verification.True(!firstTick.PlanningCompleted, "observer first tick has no completed cycle"); Verification.True(firstTick.ShouldLog, "observer first cadence tick is eligible for session-guarded logging"); Verification.Equal(10L, firstTick.Observation.VehicleState.SequenceId, "observer first tick uses first fresh state"); TrajectoryObservationLoopTick? initialPublicationTick = null; bool initiallyPublished = SpinWait.SpinUntil(() => { var currentState = new VehicleMotionState( new Pose2D(0.15d, 0d, 0d), 0.15d, null, startedAt.AddSeconds(0.10d), 11L); initialPublicationTick = loop.Tick(startedAt.AddSeconds(0.10d), currentState, CancellationToken.None); return !initialPublicationTick.PlanningInFlight; }, TimeSpan.FromSeconds(5d)); Verification.True(initiallyPublished && initialPublicationTick != null && ReferenceEquals(published, initialPublicationTick.Observation.PublishedTrajectory), "observer establishes a published trajectory before delayed rolling planning"); DateTimeOffset replanAt = startedAt.AddSeconds(settings.ReplanPeriodSeconds); var replanState = new VehicleMotionState( new Pose2D(0.20d, 0d, 0d), 0.20d, null, replanAt, 12L); TrajectoryObservationLoopTick replanTick = loop.Tick(replanAt, replanState, CancellationToken.None); Verification.True(replanTick.PlanningInFlight, "observer delayed rolling planning remains in flight after replan tick"); Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)), "observer delayed planner enters planning service"); DateTimeOffset secondAt = replanAt.AddSeconds(settings.ObserverPeriodSeconds); var secondState = new VehicleMotionState( new Pose2D(0.25d, 0d, 0d), 0.25d, null, secondAt, 13L); TrajectoryObservationLoopTick secondTick = loop.Tick(secondAt, secondState, CancellationToken.None); Verification.True(secondTick.PlanningInFlight, "observer second tick does not wait for delayed planning"); Verification.True(secondTick.ShouldLog, "observer second cadence tick is eligible for session-guarded logging"); Verification.True(ReferenceEquals(published, secondTick.Observation.PublishedTrajectory), "observer keeps observing the existing publication during delayed rolling planning"); Verification.Equal(13L, secondTick.Observation.VehicleState.SequenceId, "observer second tick uses second fresh state"); Verification.Equal(secondAt, secondTick.Observation.ObservedAtUtc, "observer second tick observes at its own time"); planningService.Release(); TrajectoryObservationLoopTick? completedTick = null; bool completed = SpinWait.SpinUntil(() => { var currentState = new VehicleMotionState( new Pose2D(0.30d, 0d, 0d), 0.30d, null, replanAt.AddSeconds(0.10d), 14L); completedTick = loop.Tick(replanAt.AddSeconds(0.10d), currentState, CancellationToken.None); return !completedTick.PlanningInFlight; }, TimeSpan.FromSeconds(5d)); Verification.True(completed, "observer delayed planning completes deterministically"); Verification.True(completedTick != null, "observer delayed planning produces a completion tick"); TrajectoryObservationLoopTick finalTick = completedTick!; Verification.True(finalTick.PlanningCompleted, "observer completion tick reports cycle completion"); Verification.True(finalTick.LatestCycle != null && finalTick.LatestCycle.Published, "observer delayed planning result is consumed without a continuation"); Verification.Equal(14L, finalTick.Observation.VehicleState.SequenceId, "observer post-plan observation does not reuse pre-plan state"); Verification.Equal(replanAt.AddSeconds(0.10d), finalTick.Observation.ObservedAtUtc, "observer post-plan observation does not reuse pre-plan time"); } finally { planningService.Release(); } } private static void VerifiesSessionLayerCleanupDecisions() { Verification.True(!TrajectoryObservationSessionLifecycle.ShouldClearLayers( TrajectoryObservationSessionEndReason.BootstrapFailure), "observer bootstrap failure preserves the diagnostic world view"); Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers( TrajectoryObservationSessionEndReason.RuntimeFault), "observer runtime fault clears all painter layers"); Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers( TrajectoryObservationSessionEndReason.Cancellation), "observer cancellation clears all painter layers"); } private static void VerifiesGearSwitchWaitStateForWorldPresentation() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 4, 0, 0, TimeSpan.Zero); EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt, EmTerminalType.GearSwitch); TrajectoryObservationRuntimeState beforeFinal = TrajectoryObservationRuntimeState.Create( effectiveAt.AddSeconds(0.99d), trajectory); Verification.True(!beforeFinal.WaitingAtGearSwitch, "observer does not paint gear-switch wait state before final time"); Verification.Equal(string.Empty, beforeFinal.WorldNotice, "observer has no gear-switch world notice before final time"); TrajectoryObservationRuntimeState atFinal = TrajectoryObservationRuntimeState.Create( effectiveAt.AddSeconds(1d), trajectory); Verification.True(atFinal.WaitingAtGearSwitch, "observer enters gear-switch wait state at final time"); 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"); string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs"); string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath)); Verification.True(presentationSource.Contains( "worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice"), "observer world painter draws the exact runtime wait state"); } private static void VerifiesLsAndStUsePublishedTrajectoryData() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero); DirectionSegmentView segment = CreateStraightSegment(); EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt); TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d); Verification.Equal(2, charts.StSamples.Count, "observer ST sample count"); Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time"); Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S"); Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time"); Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S"); Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count"); Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity, "observer first signed speed"); Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity, "observer second signed speed"); Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count"); Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count"); Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S"); Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset"); var settings = new TrajectoryObservationSettings { MaximumOsqpIterations = 9000, TimeHorizonSeconds = 4d, OutputTimeStepSeconds = 0.20d, }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper( new CoarsePathPlanningService(), new PathSmoothingService()) .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds"); var planningService = new FixedTrajectoryPlanningService(trajectory); var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check"); var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L); PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None) .GetAwaiter().GetResult(); Verification.True(firstCycle.Published, "observer first cycle publishes"); Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero"); Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId, "observer first trajectory identity"); Verification.NearlyEqual(settings.ReplanPeriodSeconds, planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds, "observer configured replan period"); Verification.NearlyEqual(settings.SolverTimeoutSeconds, planningService.Requests[0].Configuration.Scheduling.SolverTimeoutSeconds, "observer configured solver timeout"); Verification.Equal(settings.MaximumOsqpIterations, planningService.Requests[0].Configuration.Solver.MaximumOsqpIterations, "observer configured OSQP iteration limit"); Verification.NearlyEqual(settings.TimeHorizonSeconds, planningService.Requests[0].Configuration.Scheduling.TimeHorizonSeconds, "observer configured ST time horizon"); Verification.NearlyEqual(settings.OutputTimeStepSeconds, planningService.Requests[0].Configuration.Scheduling.OutputTimeStepSeconds, "observer configured ST timestamp spacing"); TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state); Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart, "observer executor interpolates from published effective time"); } private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings(); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer rolling snapshot bootstrap succeeds"); EmTrajectory published = CreatePublishedTrajectory(effectiveAt); var planningService = new FixedTrajectoryPlanningService(published); var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "snapshot-check"); var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 2L); controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult(); controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None) .GetAwaiter().GetResult(); EmPlanningRequest rollingRequest = planningService.Requests[1]; Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory), "observer rolling request uses the controlled published trajectory object"); Verification.Equal(rollingRequest.PreviousTrajectory.Metadata.TrajectoryId, rollingRequest.PreviousTrajectoryId, "observer rolling request trajectory object and ID use one publication snapshot"); } private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings(); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); var controller = new TrajectoryObservationController(bootstrap, settings, new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check"); var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L); controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult(); TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state); TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build( observation.PublishedTrajectory, CreateStraightSegment(), 0.5d); string text = TrajectoryObservationPresentationText.Create(observation, charts); Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."), "observer presentation observe-only notice"); Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"), "observer presentation selected point time and path-S"); Verification.True(text.Contains("signed speed=0.30 m/s"), "observer presentation signed speed"); Verification.True(text.Contains("yaw rate=0.00 rad/s"), "observer presentation yaw rate"); Verification.True(text.Contains("LS projection failures=0"), "observer presentation LS projection failures"); } private static void VerifiesPlanningDiagnosticsKeepRawFailureReason() { var failed = new PlanningCycleResult( 4L, new PlanningCycleIdentity(3L, "diagnostic-reference", 7L, string.Empty, 0), new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null, "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor"), false, "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor"); TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create( failed, TimeSpan.FromMilliseconds(18d), false, null); Verification.True(diagnostic.Text.Contains("cycle=4"), "diagnostic has cycle version"); Verification.True(diagnostic.Text.Contains("status=CorridorInfeasible"), "diagnostic preserves raw status"); Verification.True(diagnostic.Text.Contains("published=False"), "diagnostic preserves publish state"); Verification.True(diagnostic.Text.Contains("elapsed=18ms"), "diagnostic preserves elapsed time"); Verification.True(diagnostic.Text.Contains("reason=map=3;reference=diagnostic-reference"), "diagnostic preserves planner failure reason"); foreach (string expected in new[] { "longitudinalMode=", "remainingToBoundary=", "minimumStoppingDistance=", "minimumStoppingDuration=", "maximumStoppedReachableDistance=", }) { Verification.True(diagnostic.Text.Contains(expected), "failure diagnostic includes " + expected); } Verification.True(!diagnostic.Text.Contains("trajectory summary:"), "failed diagnostic has no stale trajectory summary"); TrajectoryObservationDiagnostic pending = TrajectoryObservationDiagnostics.Create( null, TimeSpan.Zero, true, null); Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion"); } private static void VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero); EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt); var succeeded = new PlanningCycleResult(9L, new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0), new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty); string text = TrajectoryObservationDiagnostics.Create( succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text; foreach (string expected in new[] { "trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s", "pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3", "longitudinalMode=RollingContinuation", "terminalSpeed=0.400m/s", "terminalAcceleration=0.000m/s2", }) { Verification.True(text.Contains(expected), "published diagnostic includes " + expected); } } private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic() { string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs"); string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath)); Verification.True(source.Contains("DrawLs(TrajectoryObservationCharts charts, string diagnosticText)"), "LS painter accepts planning diagnostic input"); Verification.True(source.Contains("DrawSt(TrajectoryObservationCharts charts, string diagnosticText)"), "ST painter accepts planning diagnostic input"); Verification.True(source.Contains("No published trajectory available for L-S chart.\\n"), "LS empty state includes diagnostic after chart label"); Verification.True(source.Contains("No published trajectory available for T-S/T-V charts.\\n"), "ST empty state includes diagnostic after chart label"); } private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis() { TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build( CreatePublishedTrajectory(new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero)), CreateStraightSegment(), 0.5d); TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts); Verification.Equal("path-S (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label"); Verification.Equal("lateral offset (m)", model.VerticalAxisLabel, "observer LS vertical axis label"); Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS, "observer LS path-S is horizontal"); Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset, "observer LS lateral offset is vertical"); } private static void FreezesBootstrapVehicleForRollingRequests() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings(); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); VehicleParameters originalVehicle = job.Vehicle; double expectedLength = originalVehicle.LengthMeters; double expectedWidth = originalVehicle.WidthMeters; double expectedMargin = originalVehicle.SafetyMarginMeters; double? expectedMaximumCurvature = originalVehicle.MaximumCurvaturePerMeter; double? expectedMinimumRadius = originalVehicle.MinimumTurningRadiusMeters; TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer vehicle snapshot bootstrap succeeds"); originalVehicle.LengthMeters = 91d; originalVehicle.WidthMeters = 92d; originalVehicle.SafetyMarginMeters = 93d; originalVehicle.MaximumCurvaturePerMeter = 94d; originalVehicle.MinimumTurningRadiusMeters = 95d; EmTrajectory published = CreatePublishedTrajectory(effectiveAt); var planningService = new FixedTrajectoryPlanningService(published); var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "vehicle-check"); var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 3L); controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult(); AssertVehicleSnapshot(planningService.Requests[0].Vehicle, expectedLength, expectedWidth, expectedMargin, expectedMaximumCurvature, expectedMinimumRadius, "mutated bootstrap job vehicle"); Verification.True(!ReferenceEquals(originalVehicle, planningService.Requests[0].Vehicle), "observer request does not retain mutable bootstrap vehicle object"); job.Vehicle = new VehicleParameters { LengthMeters = 101d, WidthMeters = 102d, SafetyMarginMeters = 103d, MaximumCurvaturePerMeter = 104d, MinimumTurningRadiusMeters = 105d, }; controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None) .GetAwaiter().GetResult(); AssertVehicleSnapshot(planningService.Requests[1].Vehicle, expectedLength, expectedWidth, expectedMargin, expectedMaximumCurvature, expectedMinimumRadius, "replaced bootstrap job vehicle"); } private static void VerifiesEmptyMapBootstrapProducesPublishableReference() { var settings = new TrajectoryObservationSettings(); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0.5d, 0.5d, 0d), new Pose2D(3.5d, 0.5d, 0d), settings, Array.Empty(), 0L); TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() .Bootstrap(job, CancellationToken.None); Verification.True(bootstrap.Succeeded, "observer empty-map bootstrap succeeds"); Verification.True(bootstrap.CoarseResult.MapResult.Succeeded, "observer empty-map bootstrap builds a successful map"); Verification.True(bootstrap.Map != null, "observer empty-map bootstrap returns a map"); Verification.Equal(PlanningStatus.Success, bootstrap.CoarseResult.PlanningResult.Status, "observer empty-map coarse planning succeeds"); PathSmoothingResult smoothedPath = bootstrap.SmoothedPath ?? throw new InvalidOperationException( "observer empty-map bootstrap has no smoothing result"); Verification.True(IsPublishableSmoothingStatus(smoothedPath.Status), "observer empty-map bootstrap returns a publishable smoothing result"); for (int index = 0; index < smoothedPath.Path.Count; index++) { Verification.True(!double.IsNaN(smoothedPath.Path[index].BodyClearance) && !double.IsInfinity(smoothedPath.Path[index].BodyClearance), "observer empty-map smoothing path has finite body clearance at " + index); } Verification.True(bootstrap.Segments.Count > 0, "observer empty-map bootstrap returns at least one direction segment"); } private static bool IsPublishableSmoothingStatus(PathSmoothingStatus status) { return status == PathSmoothingStatus.Complete || status == PathSmoothingStatus.PartialImprovement || status == PathSmoothingStatus.NotNeeded || status == PathSmoothingStatus.Unchanged; } private static void AssertVehicleSnapshot(VehicleParameters actual, double expectedLength, double expectedWidth, double expectedMargin, double? expectedMaximumCurvature, double? expectedMinimumRadius, string name) { Verification.NearlyEqual(expectedLength, actual.LengthMeters, name + " length"); Verification.NearlyEqual(expectedWidth, actual.WidthMeters, name + " width"); Verification.NearlyEqual(expectedMargin, actual.SafetyMarginMeters, name + " safety margin"); Verification.Equal(expectedMaximumCurvature, actual.MaximumCurvaturePerMeter, name + " maximum curvature"); Verification.Equal(expectedMinimumRadius, actual.MinimumTurningRadiusMeters, name + " minimum turning radius"); } private static DirectionSegmentView CreateStraightSegment() { var points = new List { new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), }; return new DirectionSegmentView(0, TravelDirection.Forward, points, new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d), new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d); } private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt, EmTerminalType terminalType = EmTerminalType.Goal) { var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L, "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType, EmLongitudinalMode.RollingContinuation); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d, TravelDirection.Forward, EmBoundaryType.None, 0d, 0d), new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d, TravelDirection.Forward, EmBoundaryType.None, 0d, 0d), }); } private sealed class FixedTrajectoryPlanningService : IEmPlanningService { private readonly EmTrajectory trajectory; public FixedTrajectoryPlanningService(EmTrajectory trajectory) { this.trajectory = trajectory; } public List Requests { get; } = new List(); public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) { Requests.Add(request); return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty); } } private sealed class DelayedTrajectoryPlanningService : IEmPlanningService { private readonly EmTrajectory trajectory; private readonly int delayedCall; private readonly ManualResetEventSlim entered = new ManualResetEventSlim(false); private readonly ManualResetEventSlim release = new ManualResetEventSlim(false); private int callCount; public DelayedTrajectoryPlanningService(EmTrajectory trajectory, int delayedCall) { this.trajectory = trajectory; this.delayedCall = delayedCall; } public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) { if (Interlocked.Increment(ref callCount) == delayedCall) { entered.Set(); release.Wait(cancellationToken); } return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty); } public bool WaitUntilEntered(TimeSpan timeout) { return entered.Wait(timeout); } public void Release() { release.Set(); } } private static bool Throws(Action action) { try { action(); return false; } catch (ArgumentOutOfRangeException) { return true; } } }