feat: log EM observation session planning configuration
This commit is contained in:
+1
@@ -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;
|
||||
|
||||
+25
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
+5
@@ -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);
|
||||
|
||||
@@ -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<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()
|
||||
{
|
||||
AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding");
|
||||
|
||||
Reference in New Issue
Block a user