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,
|
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;
|
||||||
|
|||||||
+25
@@ -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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
+5
@@ -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");
|
||||||
|
|||||||
Reference in New Issue
Block a user