diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index 8331162..4cdc8e7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -271,6 +271,7 @@ internal static class TrajectoryObservationMovementTestRunner var controller = new TrajectoryObservationController(bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()), "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture)); + LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text); var observationLoop = new TrajectoryObservationLoop(controller); DirectionSegmentView segment = bootstrap.Segments[0]; bool gearSwitchWaitingPrinted = false; diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs index b3a85d2..68053c0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs @@ -16,6 +16,26 @@ public sealed class TrajectoryObservationDiagnostic public static class TrajectoryObservationDiagnostics { + public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration) + { + if (configuration == null || configuration.Scheduling == null || configuration.Solver == null) + throw new ArgumentNullException(nameof(configuration)); + + double horizon = configuration.Scheduling.TimeHorizonSeconds; + double outputTimeStep = configuration.Scheduling.OutputTimeStepSeconds; + int trajectoryKnots = checked((int)Math.Ceiling(horizon / outputTimeStep) + 1); + return new TrajectoryObservationDiagnostic( + "planning configuration:\n" + + "timeHorizon=" + Format(horizon, "F2") + "s\n" + + "distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" + + "outputTimeStep=" + Format(outputTimeStep, "F2") + "s\n" + + "outputFrequency=" + Format(1d / outputTimeStep, "F2") + "Hz\n" + + "trajectoryKnots=" + trajectoryKnots.ToString(CultureInfo.InvariantCulture) + "\n" + + "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" + + "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" + + "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s"); + } + public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle, TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory) { @@ -37,4 +57,9 @@ public static class TrajectoryObservationDiagnostics text += "\nreason=" + latestCycle.Result.FailureReason; return new TrajectoryObservationDiagnostic(text); } + + private static string Format(double value, string format) + { + return value.ToString(format, CultureInfo.InvariantCulture); + } } diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index 74fe8b2..93ac075 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -218,6 +218,11 @@ public sealed class TrajectoryObservationController public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory; + public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic() + { + return TrajectoryObservationDiagnostics.CreateConfiguration(configuration); + } + public bool ShouldStartCycle(DateTimeOffset now) { return coordinator.ShouldStartCycle(now); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 5a9ad38..77034b2 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -21,6 +21,7 @@ internal static class TrajectoryObservationChecks VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesOperatorDocumentationUsesExactUiEntry(); VerifiesMovementTestVehicleInputsAndSettingsSnapshot(); + VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration(); RejectsInvalidObservationSettings(); VerifiesStartGoalBoundsUseOnlyConfiguredPadding(); VerifiesValidNonEmptyObstacleSource(); @@ -106,14 +107,14 @@ internal static class TrajectoryObservationChecks "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 = 0.50d;"), - "observer MovementTest exposes the test solver timeout default"); - Verification.True(source.Contains("public int MaximumOsqpIterations = 12000;"), - "observer MovementTest exposes the test OSQP iteration default"); - Verification.True(source.Contains("public double TimeHorizonSeconds = 6d;"), - "observer MovementTest exposes the ST time-horizon default"); - Verification.True(source.Contains("public double OutputTimeStepSeconds = 0.10d;"), - "observer MovementTest exposes the ST timestamp-spacing 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,"), @@ -190,6 +191,46 @@ internal static class TrajectoryObservationChecks "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", + }) + { + 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");