feat: log EM observation session planning configuration

This commit is contained in:
梁薄云
2026-08-05 15:32:37 +08:00
parent 32083db4d0
commit 9b59ce4a71
4 changed files with 80 additions and 8 deletions
@@ -271,6 +271,7 @@ internal static class TrajectoryObservationMovementTestRunner
var controller = new TrajectoryObservationController(bootstrap, settings, var controller = new TrajectoryObservationController(bootstrap, settings,
new EmPlanningService(new OsqpNativeSolver()), new EmPlanningService(new OsqpNativeSolver()),
"trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture)); "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
var observationLoop = new TrajectoryObservationLoop(controller); var observationLoop = new TrajectoryObservationLoop(controller);
DirectionSegmentView segment = bootstrap.Segments[0]; DirectionSegmentView segment = bootstrap.Segments[0];
bool gearSwitchWaitingPrinted = false; bool gearSwitchWaitingPrinted = false;
@@ -16,6 +16,26 @@ public sealed class TrajectoryObservationDiagnostic
public static class TrajectoryObservationDiagnostics 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, public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory) TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory)
{ {
@@ -37,4 +57,9 @@ public static class TrajectoryObservationDiagnostics
text += "\nreason=" + latestCycle.Result.FailureReason; text += "\nreason=" + latestCycle.Result.FailureReason;
return new TrajectoryObservationDiagnostic(text); return new TrajectoryObservationDiagnostic(text);
} }
private static string Format(double value, string format)
{
return value.ToString(format, CultureInfo.InvariantCulture);
}
} }
@@ -218,6 +218,11 @@ public sealed class TrajectoryObservationController
public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory; public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
{
return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
}
public bool ShouldStartCycle(DateTimeOffset now) public bool ShouldStartCycle(DateTimeOffset now)
{ {
return coordinator.ShouldStartCycle(now); return coordinator.ShouldStartCycle(now);
@@ -21,6 +21,7 @@ internal static class TrajectoryObservationChecks
VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesObservationSourceUsesRequiredOperatorText();
VerifiesOperatorDocumentationUsesExactUiEntry(); VerifiesOperatorDocumentationUsesExactUiEntry();
VerifiesMovementTestVehicleInputsAndSettingsSnapshot(); VerifiesMovementTestVehicleInputsAndSettingsSnapshot();
VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration();
RejectsInvalidObservationSettings(); RejectsInvalidObservationSettings();
VerifiesStartGoalBoundsUseOnlyConfiguredPadding(); VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
VerifiesValidNonEmptyObstacleSource(); VerifiesValidNonEmptyObstacleSource();
@@ -106,14 +107,14 @@ internal static class TrajectoryObservationChecks
"observer MovementTest exposes safety margin with Task-1 default"); "observer MovementTest exposes safety margin with Task-1 default");
Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"), Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"),
"observer MovementTest exposes maximum curvature with Task-1 default"); "observer MovementTest exposes maximum curvature with Task-1 default");
Verification.True(source.Contains("public double SolverTimeoutSeconds = 0.50d;"), Verification.True(source.Contains("public double SolverTimeoutSeconds ="),
"observer MovementTest exposes the test solver timeout default"); "observer MovementTest exposes a configurable solver timeout");
Verification.True(source.Contains("public int MaximumOsqpIterations = 12000;"), Verification.True(source.Contains("public int MaximumOsqpIterations ="),
"observer MovementTest exposes the test OSQP iteration default"); "observer MovementTest exposes a configurable OSQP iteration limit");
Verification.True(source.Contains("public double TimeHorizonSeconds = 6d;"), Verification.True(source.Contains("public double TimeHorizonSeconds ="),
"observer MovementTest exposes the ST time-horizon default"); "observer MovementTest exposes a configurable ST time horizon");
Verification.True(source.Contains("public double OutputTimeStepSeconds = 0.10d;"), Verification.True(source.Contains("public double OutputTimeStepSeconds ="),
"observer MovementTest exposes the ST timestamp-spacing default"); "observer MovementTest exposes a configurable ST timestamp spacing");
Verification.True(source.Contains("不等于观察循环周期"), Verification.True(source.Contains("不等于观察循环周期"),
"observer MovementTest documents the distinction between output timestamps and observer ticks"); "observer MovementTest documents the distinction between output timestamps and observer ticks");
Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"), Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"),
@@ -190,6 +191,46 @@ internal static class TrajectoryObservationChecks
"observer configured bootstrap vehicle"); "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<TrajectoryObservationObstacle>(), 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() private static void RejectsInvalidObservationSettings()
{ {
AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding"); AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding");