From d378ad62571ecab6b30270999c4f606275b97418 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 15:51:31 +0800 Subject: [PATCH] fix: stabilize stopped-start EM planning and add txt diagnostics --- .../Configuration/EmPlannerConfiguration.cs | 6 +- .../SequentialLongitudinalOptimizer.cs | 78 ++++++++++++++----- .../ParkrobTrajplanner/EMPlanner/README.md | 6 +- .../Validation/EmPlanningRequestValidator.cs | 4 +- .../MovementTest.TrajectoryObservationTest.cs | 64 ++++++++++++++- .../tarjplanner_movementtest/README.md | 7 +- .../TrajectoryObservationDiagnostics.cs | 75 +++++++++++++++++- .../TrajectoryObservationReportWriter.cs | 67 ++++++++++++++++ .../FoundationChecks.cs | 8 +- .../LongitudinalIntegrationChecks.cs | 37 +++++++++ .../LongitudinalModelChecks.cs | 6 +- .../TrajectoryChecks.cs | 24 +++--- .../TrajectoryObservationChecks.cs | 44 +++++++++++ 13 files changed, 373 insertions(+), 53 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationReportWriter.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs index 5036dc7..6bcbb34 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs @@ -96,9 +96,9 @@ public sealed partial class EmPlannerConfiguration Validation = new ValidationConfiguration { SpatialToleranceMeters = 1e-8d, - KinematicTolerance = 1e-8d, - TerminalPositionToleranceMeters = 0.03d, - TerminalYawToleranceRadians = 5d * System.Math.PI / 180d, + KinematicTolerance = 1e-5d, + TerminalPositionToleranceMeters = 0d, + TerminalYawToleranceRadians = 0d, }, }; } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs index ec04e82..1760ce3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; +using System.Globalization; using System.Threading; using MultiWheelC.TrajectoryPlanning.CoarsePath; @@ -38,6 +39,21 @@ public sealed class SequentialLongitudinalOptimizer return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required."); if (cancellationToken.IsCancellationRequested) return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled."); + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && + input.Mode == EmLongitudinalMode.ExactStopAtBoundary) + { + double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters, + input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond); + if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance && + Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <= + input.Configuration.Validation.KinematicTolerance) + { + input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d, + input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode, + input.Configuration, input.PlanningScope, input.KnotSchedule, + input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond); + } + } if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance, out int iterationLimit, out string configurationFailure)) { @@ -223,14 +239,27 @@ public sealed class SequentialLongitudinalOptimizer projectionSolveCount = 0; failureStatus = EmPlanningStatus.LongitudinalInfeasible; failureReason = string.Empty; - if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters && + double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters, + input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond); + bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance && Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <= - input.Configuration.Validation.KinematicTolerance && - TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed)) + input.Configuration.Validation.KinematicTolerance; + bool staticStartSeedUsed = false; + string staticStartSeedFailure = string.Empty; + if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed, + out staticStartSeedFailure)) { + staticStartSeedUsed = true; candidate = staticStartSeed; return true; } + string WithStaticSeedDiagnostic(string reason) + { + return staticStartEligible && !staticStartSeedUsed + ? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason + : reason; + } + LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input); string lastRejection = string.Empty; for (int iteration = 0; iteration < iterationLimit; iteration++) @@ -238,21 +267,21 @@ public sealed class SequentialLongitudinalOptimizer if (cancellationToken.IsCancellationRequested) { failureStatus = EmPlanningStatus.Cancelled; - failureReason = "Initial full-direction feasibility projection was cancelled."; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled."); return false; } TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed; if (remainingBudget <= TimeSpan.Zero) { failureStatus = EmPlanningStatus.SolverTimedOut; - failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget."; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the shared solve budget."); return false; } if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate, out QuadraticProgram problem, out string buildFailure)) { failureStatus = EmPlanningStatus.LongitudinalInfeasible; - failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure); return false; } @@ -267,45 +296,45 @@ public sealed class SequentialLongitudinalOptimizer if (solved == null) { failureStatus = EmPlanningStatus.Failed; - failureReason = "The initial full-direction feasibility solver returned no result."; + failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result."); return false; } if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations) { failureStatus = EmPlanningStatus.SolverTimedOut; - failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus + + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus + ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" + - solved.DualResidual + "): " + solved.Diagnostic; + solved.DualResidual + "): " + solved.Diagnostic); return false; } if (solved.Status == QpSolveStatus.Cancelled) { failureStatus = EmPlanningStatus.Cancelled; - failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic); return false; } if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible) { failureStatus = EmPlanningStatus.LongitudinalInfeasible; - failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic); return false; } if (solved.Status == QpSolveStatus.SolverUnavailable) { failureStatus = EmPlanningStatus.SolverUnavailable; - failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic); return false; } if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate) { failureStatus = EmPlanningStatus.Failed; - failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic); return false; } if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected)) { failureStatus = EmPlanningStatus.LongitudinalInfeasible; - failureReason = "Initial full-direction feasibility solver primal does not match the ST layout."; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout."); return false; } if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance)) @@ -319,7 +348,7 @@ public sealed class SequentialLongitudinalOptimizer if (validationStatus == EmPlanningStatus.NoProgress) { failureStatus = validationStatus; - failureReason = validationFailure; + failureReason = WithStaticSeedDiagnostic(validationFailure); return false; } lastRejection = validationFailure; @@ -329,7 +358,7 @@ public sealed class SequentialLongitudinalOptimizer out LongitudinalCandidate nextLinearization)) { failureStatus = EmPlanningStatus.LongitudinalInfeasible; - failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope."; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope."); return false; } linearizationIterate = nextLinearization; @@ -339,8 +368,8 @@ public sealed class SequentialLongitudinalOptimizer lastRejection = "Initial feasibility projection violated the strict physical validator."; } failureStatus = EmPlanningStatus.LongitudinalInfeasible; - failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " + - lastRejection; + failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " + + lastRejection); return false; } @@ -466,24 +495,31 @@ public sealed class SequentialLongitudinalOptimizer } private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, - out LongitudinalCandidate candidate) + out LongitudinalCandidate candidate, out string failureReason) { candidate = null; + failureReason = "unknown"; int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex; if (stabilizationStart < 5) + { + failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture); return false; + } IReadOnlyList times = input.KnotSchedule.KnotTimes; if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate)) return true; + failureReason = "exactJerkSeed=failed"; double firstDuration = times[1] - times[0]; double secondDuration = times[2] - times[1]; LongitudinalConfiguration configuration = input.Configuration.Longitudinal; double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed, Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration, configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration)); - for (int sample = 1; sample <= 256; sample++) + for (int sample = -256; sample <= 256; sample++) { + if (sample == 0) + continue; double firstJerk = maximumFirstJerk * sample / 256d; var jerk = new double[stabilizationStart]; jerk[0] = firstJerk; @@ -497,9 +533,11 @@ public sealed class SequentialLongitudinalOptimizer out LongitudinalCandidate strict, out _)) { candidate = strict; + failureReason = string.Empty; return true; } } + failureReason = "exactJerkSeed=failed; sampledSeeds=failed"; return false; } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md index 17567d5..64f2f36 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md @@ -94,8 +94,8 @@ LS 的变量是 `l`、`dl`、`ddl` 和区间 `dddl`；它对静态走廊、导 | 静态走廊、LS、ST 或停车条件不可行 | `CorridorInfeasible`、`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient` | 空 | 不发布；由上层决定重试、停车或重新选路 | | 求解器不可用、超时、取消或发布复核失败 | `SolverUnavailable`、`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Failed` | 空 | 读取 `FailureReason`；不得使用中间轨迹 | | 已被更新周期替代 | `Superseded` | 空 | 由滚动协调器忽略旧结果 | -| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点容差内或受监督停车保持观察可合法零进度 | -| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0.03 m` 或归一化 yaw 误差超过 `5°` | +| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点精确匹配或受监督停车保持观察可合法零进度 | +| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0 m` 或归一化 yaw 误差超过 `0 rad` | | 自适应 knots 或发布样本超限 | `FullSegmentResourceLimitExceeded` | 空 | 不发布；不得截断轨迹或回退滚动模式 | `FailureReason` 始终以 `map=...;reference=...;state=...;previous=...;segment=...` 开头，供调用方追踪地图、参考路径、状态、上一轨迹和方向段绑定。 @@ -165,7 +165,7 @@ EmTrajectory trajectory = result.Trajectory; - 对完整活动方向段做一次优化；`s_end` 来自已选 Local G2 的实际 `PathS`，`T_end` 由可行加速度、巡航、jerk 限速停止和零速 hold 推导，不依赖旧的距离/时间截断窗口。 - 前进期望/硬上限为 `1.0 m/s`，倒车期望/硬上限为 `0.5 m/s`。 -- 成功终端必须停止，世界位置误差不超过 `0.03 m`，归一化 yaw 误差不超过 `5°`（约 `0.0873 rad`）。 +- 成功终端必须停止，世界位置误差和归一化 yaw 误差均为 `0`（精确到达目标）。 - `NoProgress`、`TerminalPoseMismatch`、`FullSegmentResourceLimitExceeded` 失败均返回空轨迹，不降级为滚动或截断发布。 - `DistanceHorizonMeters` 与 `TimeHorizonSeconds` 作为兼容字段保留，但不用于截断 full 模式。 diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs index ebd9242..80387e2 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs @@ -150,8 +150,8 @@ public static class EmPlanningRequestValidator !Positive(scheduling.MaximumOptimizationTimeStepSeconds) || !Positive(scheduling.MaximumOptimizationSpatialStepMeters) || scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 || - !Positive(validation.TerminalPositionToleranceMeters) || - !Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI || + validation.TerminalPositionToleranceMeters < 0d || + validation.TerminalYawToleranceRadians < 0d || validation.TerminalYawToleranceRadians > Math.PI || !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights)) return false; diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index 6b877cb..5eae16b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -204,6 +204,7 @@ internal static class TrajectoryObservationMovementTestRunner private static Task activeTask; private static TrajectoryObservationVisualizationPublisher activeWebPublisher; private static TrajectoryObservationPresentation activePresentation; + private static TrajectoryObservationReportWriter activeReportWriter; private static long nextSessionId; private static long activeSessionId; private static long stateSequence; @@ -218,6 +219,7 @@ internal static class TrajectoryObservationMovementTestRunner var cancellation = new CancellationTokenSource(); CancellationTokenSource previousCancellation; long sessionId; + TrajectoryObservationReportWriter reportWriter = null; lock (SessionSync) { previousCancellation = activeCancellation; @@ -227,6 +229,8 @@ internal static class TrajectoryObservationMovementTestRunner activeSessionId = sessionId; StopActiveVisualizationNoLock(); activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null; + reportWriter = new TrajectoryObservationReportWriter(sessionId); + activeReportWriter = reportWriter; PrintStatus("Starting frozen observation session " + sessionId.ToString(CultureInfo.InvariantCulture) + "."); } @@ -239,7 +243,7 @@ internal static class TrajectoryObservationMovementTestRunner { if (activeSessionId == sessionId) activeTask = task; } - _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed), TaskScheduler.Default); + _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed, reportWriter), TaskScheduler.Default); } internal static void Stop() @@ -271,6 +275,13 @@ internal static class TrajectoryObservationMovementTestRunner { token.ThrowIfCancellationRequested(); VehicleMotionState initialState = ReadVehicleState(); + LogIfCurrent(sessionId, "initial state: pose=(" + Format(initialState.Pose.X) + ", " + + Format(initialState.Pose.Y) + ", " + Format(initialState.Pose.Heading) + ") m/rad, signed speed=" + + initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + " m/s, acceleration=" + + (initialState.LongitudinalAccelerationMetersPerSecondSquared.HasValue + ? Format(initialState.LongitudinalAccelerationMetersPerSecondSquared.Value) + : "unavailable") + " m/s2, stateSequence=" + + initialState.SequenceId.ToString(CultureInfo.InvariantCulture)); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion); @@ -294,6 +305,15 @@ internal static class TrajectoryObservationMovementTestRunner new EmPlanningService(new OsqpNativeSolver()), "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture)); LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text); + LogIfCurrent(sessionId, CreateSettingsDiagnostic(settings)); + EmPlannerConfiguration effectiveConfiguration = controller.CreateEffectiveConfigurationSnapshot(); + double stopSpeedTolerance = effectiveConfiguration.Longitudinal.StopSpeedToleranceMetersPerSecond; + LogIfCurrent(sessionId, "static start check: initialSignedSpeed=" + + initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + + ", stopSpeedTolerance=" + stopSpeedTolerance.ToString("R", CultureInfo.InvariantCulture) + + ", treatedAsStopped=" + + (Math.Abs(initialState.SignedLongitudinalSpeedMetersPerSecond) <= stopSpeedTolerance) + .ToString(CultureInfo.InvariantCulture)); var observationLoop = new TrajectoryObservationLoop(controller); TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled( sessionId, bootstrap, controller, settings, obstacleSnapshotVersion); @@ -332,7 +352,14 @@ internal static class TrajectoryObservationMovementTestRunner if (tick.PlanningStarted) LogIfCurrent(sessionId, "planning status=pending"); if (tick.PlanningCompleted) + { LogIfCurrent(sessionId, diagnostic.Text); + if (observation.PublishedTrajectory != null && charts != null) + { + AppendReportIfCurrent(sessionId, + TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text); + } + } } } @@ -490,14 +517,25 @@ internal static class TrajectoryObservationMovementTestRunner } } + private static void AppendReportIfCurrent(long sessionId, string text) + { + lock (SessionSync) + { + if (activeSessionId != sessionId) return; + activeReportWriter?.Append(text); + } + } + private static void PrintStatus(string message) { string text = ObserveOnlyNotice + "\n" + message; Hedingben.ToastText(text, StatusChannel); Console.WriteLine("[TrajectoryObserver] " + text); + activeReportWriter?.Append(message); } - private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed) + private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed, + TrajectoryObservationReportWriter reportWriter) { try { @@ -514,16 +552,38 @@ internal static class TrajectoryObservationMovementTestRunner { lock (SessionSync) { + if (ReferenceEquals(activeReportWriter, reportWriter)) + activeReportWriter = null; if (activeSessionId == sessionId) { activeTask = null; activeCancellation = null; } } + try + { + string reportPath = reportWriter.Save(); + Console.WriteLine("[TrajectoryObserver] report=" + reportPath); + } + catch (Exception exception) + { + Console.WriteLine("[TrajectoryObserver] report write failed: " + exception.Message); + } cancellation.Dispose(); } } + private static string CreateSettingsDiagnostic(TrajectoryObservationSettings settings) + { + return "observation settings: planningScope=" + settings.PlanningScope + + ", webVisualization=" + settings.EnableWebVisualization + + ", nativePainter=" + settings.EnableNativePainterVisualization + + ", outputTimeStep=" + Format(settings.OutputTimeStepSeconds) + "s" + + ", observerPeriod=" + Format(settings.ObserverPeriodSeconds) + "s" + + ", solverTimeout=" + Format(settings.SolverTimeoutSeconds) + "s" + + ", maximumOsqpIterations=" + settings.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture); + } + private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception) { lock (SessionSync) diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md index f052806..2df4649 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md @@ -10,6 +10,10 @@ diagnostic prediction only. Goal and rolling-safety-stop commands are logged, ne gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation; it does not create or dispatch a direction-change action. +Every frozen observation session writes a text report to +`TrajectoryObservationReports/trajectory-observation-session-.txt` in the process working directory when the +session stops; the report path is printed to the console. + ## FullDirectionSegment observation (default) `UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen @@ -20,8 +24,7 @@ and zero-speed hold behavior instead of being forced by a legacy time horizon. `DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary -requires a terminal stop plus world-position error no greater than `0.03 m` and normalized yaw error no greater than -`5 deg` (`5 * PI / 180 rad`). +requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`). ## Rolling trajectory observation diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs index 649dce2..b8074ea 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Globalization; +using System.Text; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -18,10 +19,69 @@ public sealed class TrajectoryObservationDiagnostic public static class TrajectoryObservationDiagnostics { + public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts, + EmTrajectory trajectory) + { + if (charts == null || trajectory == null) + return new TrajectoryObservationDiagnostic("curve report unavailable"); + + IReadOnlyList points = trajectory.Points; + var text = new StringBuilder(); + text.AppendLine("LS_CURVE"); + text.AppendLine("index,referenceS(m),lateralOffset(m)"); + for (int index = 0; index < charts.LsSamples.Count; index++) + { + TrajectoryObservationLsSample sample = charts.LsSamples[index]; + text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," + + sample.PathS.ToString("R", CultureInfo.InvariantCulture) + "," + + sample.LateralOffset.ToString("R", CultureInfo.InvariantCulture)); + } + + text.AppendLine("ST_CURVE"); + text.AppendLine("index,time(s),pathS(m)"); + for (int index = 0; index < charts.StSamples.Count; index++) + { + TrajectoryObservationStSample sample = charts.StSamples[index]; + text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," + + sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," + + sample.PathS.ToString("R", CultureInfo.InvariantCulture)); + } + + text.AppendLine("SPEED_CURVE"); + text.AppendLine("index,time(s),signedSpeed(m/s)"); + for (int index = 0; index < charts.SpeedSamples.Count; index++) + { + TrajectoryObservationSpeedSample sample = charts.SpeedSamples[index]; + text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," + + sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," + + sample.SignedLongitudinalVelocity.ToString("R", CultureInfo.InvariantCulture)); + } + + text.AppendLine("ACCELERATION_CURVE"); + text.AppendLine("index,time(s),acceleration(m/s2)"); + for (int index = 0; index < points.Count; index++) + { + text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," + + points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," + + points[index].LongitudinalAcceleration.ToString("R", CultureInfo.InvariantCulture)); + } + + text.AppendLine("JERK_CURVE"); + text.AppendLine("index,time(s),jerk(m/s3)"); + for (int index = 0; index + 1 < points.Count; index++) + { + text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," + + points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," + + points[index].LongitudinalJerk.ToString("R", CultureInfo.InvariantCulture)); + } + + return new TrajectoryObservationDiagnostic(text.ToString()); + } + public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration) { if (configuration == null || configuration.Scheduling == null || configuration.Solver == null || - configuration.Longitudinal == null) + configuration.Longitudinal == null || configuration.Validation == null) throw new ArgumentNullException(nameof(configuration)); double horizon = configuration.Scheduling.TimeHorizonSeconds; @@ -57,6 +117,19 @@ public static class TrajectoryObservationDiagnostics "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" + "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" + "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s\n" + + "kinematicTolerance=" + Format(configuration.Validation.KinematicTolerance, "G17") + "\n" + + "stopSpeedToleranceMetersPerSecond=" + Format(longitudinal.StopSpeedToleranceMetersPerSecond, "G17") + "\n" + + "terminalPositionToleranceMeters=" + Format(configuration.Validation.TerminalPositionToleranceMeters, "G17") + "\n" + + "terminalYawToleranceRadians=" + Format(configuration.Validation.TerminalYawToleranceRadians, "G17") + "\n" + + "maximumOptimizationTimeStepSeconds=" + Format(configuration.Scheduling.MaximumOptimizationTimeStepSeconds, "F3") + "\n" + + "maximumOptimizationSpatialStepMeters=" + Format(configuration.Scheduling.MaximumOptimizationSpatialStepMeters, "F3") + "\n" + + "maximumOptimizationKnotCount=" + configuration.Scheduling.MaximumOptimizationKnotCount.ToString(CultureInfo.InvariantCulture) + "\n" + + "maximumPublishedSampleCount=" + configuration.Scheduling.MaximumPublishedSampleCount.ToString(CultureInfo.InvariantCulture) + "\n" + + "maximumForwardSpeedMetersPerSecond=" + Format(longitudinal.MaximumForwardSpeedMetersPerSecond, "F3") + "\n" + + "maximumReverseSpeedMetersPerSecond=" + Format(longitudinal.MaximumReverseSpeedMetersPerSecond, "F3") + "\n" + + "maximumAccelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumAccelerationMetersPerSecondSquared, "F3") + "\n" + + "maximumDecelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumDecelerationMetersPerSecondSquared, "F3") + "\n" + + "maximumJerkMetersPerSecondCubed=" + Format(longitudinal.MaximumJerkMetersPerSecondCubed, "F3") + "\n" + "maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" + "maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" + "requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m"); diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationReportWriter.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationReportWriter.cs new file mode 100644 index 0000000..8b29b82 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationReportWriter.cs @@ -0,0 +1,67 @@ +using System; +using System.Globalization; +using System.IO; +using System.Text; + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +/// Buffers a readable text report for one trajectory observation session. +public sealed class TrajectoryObservationReportWriter +{ + private readonly long _sessionId; + private readonly string _reportDirectory; + private readonly StringBuilder _content = new StringBuilder(); + private readonly object _sync = new object(); + + public TrajectoryObservationReportWriter(long sessionId, string reportDirectory = null) + { + if (sessionId <= 0) + throw new ArgumentOutOfRangeException(nameof(sessionId)); + + _sessionId = sessionId; + _reportDirectory = reportDirectory ?? + Path.Combine(Environment.CurrentDirectory, "TrajectoryObservationReports"); + AppendHeader(); + } + + public string FilePath + { + get + { + return Path.Combine(_reportDirectory, "trajectory-observation-session-" + + _sessionId.ToString(CultureInfo.InvariantCulture) + ".txt"); + } + } + + public void Append(string text) + { + if (string.IsNullOrEmpty(text)) + return; + + lock (_sync) + { + _content.Append(DateTimeOffset.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture)); + _content.Append(" | "); + _content.AppendLine(text); + } + } + + public string Save() + { + lock (_sync) + { + Directory.CreateDirectory(_reportDirectory); + File.WriteAllText(FilePath, _content.ToString(), new UTF8Encoding(false)); + return FilePath; + } + } + + private void AppendHeader() + { + _content.AppendLine("EM trajectory observation report"); + _content.AppendLine("session=" + _sessionId.ToString(CultureInfo.InvariantCulture)); + _content.AppendLine("OBSERVE_ONLY: no chassis command is sent."); + _content.AppendLine("reportGeneratedUtc=" + DateTimeOffset.UtcNow.ToString("O", CultureInfo.InvariantCulture)); + _content.AppendLine(); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs index eeecb2e..ccc5b08 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs @@ -101,6 +101,8 @@ internal static class FoundationChecks EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance"); EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance"); EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance"); + EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Validation.KinematicTolerance, + "kinematic tolerance"); EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start"); EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish"); EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose"); @@ -179,9 +181,9 @@ internal static class FoundationChecks "optimization knot cap"); EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount, "publication sample cap"); - EMPlannerVerificationHost.Verification.NearlyEqual(0.03d, + EMPlannerVerificationHost.Verification.NearlyEqual(0d, defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance"); - EMPlannerVerificationHost.Verification.NearlyEqual(5d * Math.PI / 180d, + EMPlannerVerificationHost.Verification.NearlyEqual(0d, defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance"); EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress, "no-progress status exists"); @@ -200,7 +202,7 @@ internal static class FoundationChecks copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed"); EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount, "copy keeps knot cap"); - EMPlannerVerificationHost.Verification.NearlyEqual(0.03d, + EMPlannerVerificationHost.Verification.NearlyEqual(0d, copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance"); EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault(); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index 26f703d..bb644ee 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -15,6 +15,7 @@ internal static class LongitudinalIntegrationChecks VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed(); VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence(); VerifiesFullDirectionStaticStartMakesProgress(); + VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed(); VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold(); VerifiesExactStopIncludesAStabilizationTail(); VerifiesLastStrictCandidateSurvivesLaterTimeout(); @@ -150,6 +151,42 @@ internal static class LongitudinalIntegrationChecks "full segment stops at the terminal boundary"); } + private static void VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.SolverTimeoutSeconds = 5d; + configuration.Solver.MaximumOsqpIterations = 100000; + LateralPath path = new LateralPath(new[] + { + Point(0d, 0d, 0d), + Point(2.5d, 2.5d, 0d), + Point(5d, 5d, 0d), + }, true); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.01d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "near-zero envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.01d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule schedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "near-zero schedule: " + failureReason); + + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.01d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, schedule, Array.Empty(), Array.Empty()); + LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input, + CancellationToken.None); + + Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback, + "near-zero full-direction start succeeds: " + result.FailureReason); + LongitudinalCandidate candidate = result.Candidate ?? + throw new InvalidOperationException("Near-zero full-direction candidate was missing."); + Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1], + "near-zero start reaches the terminal boundary"); + Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], + "near-zero start stops at the terminal boundary"); + } + private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold() { EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index f4aa36d..4dce759 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -769,9 +769,9 @@ internal static class LongitudinalModelChecks out speedFailure); Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure); Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _, - out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract"); - Verification.True(failureStatus != EmPlanningStatus.NoProgress, - "near-terminal gear-switch stop is not classified as no progress"); + out failureStatus, out _), "near-terminal stationary full segment is rejected"); + Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, + "near-terminal gear-switch stop is classified as no progress"); } private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically() diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs index 555de2f..c3634b3 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -194,31 +194,27 @@ internal static class TrajectoryChecks EmTrajectoryPoint terminal = valid.Points[terminalIndex]; var validator = new EmTrajectoryValidator(); - EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle, - context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw), + EmTrajectoryValidationResult exactPosition = validator.Validate(valid, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, terminal.Yaw), EmBoundaryType.Goal); - Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted"); + Verification.True(exactPosition.IsValid, "exact terminal position is accepted"); EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle, - context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw), + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.001d, terminal.Y, terminal.Yaw), EmBoundaryType.Goal); Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure, - "3.1-centimetre terminal position error is rejected"); + "1-millimetre terminal position error is rejected"); double degrees = Math.PI / 180d; - EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees)); + EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: Math.PI)); EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle, - context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees), + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -Math.PI), EmBoundaryType.Goal); - Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error"); - EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle, - context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees), - EmBoundaryType.Goal); - Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted"); + Verification.True(wrapped.IsValid, "PI and -PI use normalized yaw error"); EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle, - context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees), + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -0.1d * degrees), EmBoundaryType.Goal); Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure, - "5.1-degree terminal yaw error is rejected"); + "0.1-degree terminal yaw error is rejected"); EmTrajectory rolling = new EmTrajectoryAssembler().Assemble( CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(), diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 6843a47..81d7aa3 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -23,6 +23,7 @@ internal static class TrajectoryObservationChecks { TrajectoryObservationSettingsChecks.Run(); TrajectoryObservationSegmentChecks.Run(); + VerifiesTrajectoryObservationReportWriterCreatesTxt(); VerifiesPainterPoseGeometryReplacesLargeArrow(); TrajectoryObservationVisualizationChecks.Run(); VerifiesObservationSourceHasNoActuatorCalls(); @@ -47,6 +48,7 @@ internal static class TrajectoryObservationChecks VerifiesPresentationTextDescribesObservationWithoutSendingCommand(); VerifiesPlanningDiagnosticsKeepRawFailureReason(); VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary(); + VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk(); VerifiesEmptyChartsReceivePersistentPlanningDiagnostic(); VerifiesLsPresentationUsesPathSOnHorizontalAxis(); VerifiesPainterStAxesUsePathSAndTimeUnits(); @@ -81,6 +83,29 @@ internal static class TrajectoryObservationChecks } } + private static void VerifiesTrajectoryObservationReportWriterCreatesTxt() + { + string reportDirectory = Path.Combine(Path.GetTempPath(), + "em-trajectory-report-" + Guid.NewGuid().ToString("N")); + try + { + var writer = new TrajectoryObservationReportWriter(17L, reportDirectory); + writer.Append("cycle=1 status=SolverTimedOut primal=140.918"); + string reportPath = writer.Save(); + + Verification.True(File.Exists(reportPath), "trajectory observation txt report exists"); + string text = File.ReadAllText(reportPath); + Verification.True(text.Contains("session=17"), "report includes session identity"); + Verification.True(text.Contains("OBSERVE_ONLY"), "report records observation-only boundary"); + Verification.True(text.Contains("SolverTimedOut"), "report includes cycle failure"); + } + finally + { + if (Directory.Exists(reportDirectory)) + Directory.Delete(reportDirectory, true); + } + } + private static void VerifiesObservationSourceUsesRequiredOperatorText() { string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", @@ -960,6 +985,25 @@ internal static class TrajectoryObservationChecks } } + private static void VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk() + { + DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero); + EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt); + DirectionSegmentView segment = CreateStraightSegment(); + TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d); + + string text = TrajectoryObservationDiagnostics.CreateCurveReport(charts, trajectory).Text; + + foreach (string section in new[] + { + "LS_CURVE", "ST_CURVE", "SPEED_CURVE", "ACCELERATION_CURVE", "JERK_CURVE", + "referenceS(m)", "time(s)", "signedSpeed(m/s)", "acceleration(m/s2)", "jerk(m/s3)", + }) + { + Verification.True(text.Contains(section), "curve report includes " + section); + } + } + private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic() { string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",