First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: configure MovementTest ST time controls

Browse Source
This commit is contained in:
梁薄云
2026-08-05 13:39:18 +08:00
parent 308c8bb587
commit ddea58a139
4 changed files with 60 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
+41 -1
View File
@@ -110,6 +110,12 @@ internal static class TrajectoryObservationChecks
"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("不等于观察循环周期"),
"observer MovementTest documents the distinction between output timestamps and observer ticks");
Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"),
"observer MovementTest copies vehicle length into settings");
Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"),
@@ -122,6 +128,10 @@ internal static class TrajectoryObservationChecks
"observer MovementTest copies solver timeout into settings");
Verification.True(source.Contains("MaximumOsqpIterations = MaximumOsqpIterations,"),
"observer MovementTest copies OSQP iteration limit into settings");
Verification.True(source.Contains("TimeHorizonSeconds = TimeHorizonSeconds,"),
"observer MovementTest copies ST time horizon into settings");
Verification.True(source.Contains("OutputTimeStepSeconds = OutputTimeStepSeconds,"),
"observer MovementTest copies ST timestamp spacing into settings");
string normalizedSource = source.Replace("\r\n", "\n");
Verification.True(normalizedSource.Contains(
"VehicleMotionState state = ReadVehicleState();\n DateTimeOffset now = state.CapturedAtUtc;"),
@@ -130,6 +140,10 @@ internal static class TrajectoryObservationChecks
"observer settings use the test solver timeout default");
Verification.Equal(12000, new TrajectoryObservationSettings().MaximumOsqpIterations,
"observer settings use the test OSQP iteration default");
Verification.NearlyEqual(6d, new TrajectoryObservationSettings().TimeHorizonSeconds,
"observer settings use the ST time-horizon default");
Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
"observer settings use the ST timestamp-spacing default");
var configured = new TrajectoryObservationSettings
{
@@ -139,6 +153,8 @@ internal static class TrajectoryObservationChecks
MaximumCurvaturePerMeter = 0.55d,
SolverTimeoutSeconds = 0.42d,
MaximumOsqpIterations = 9000,
TimeHorizonSeconds = 4d,
OutputTimeStepSeconds = 0.20d,
};
TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot();
configured.VehicleLengthMeters = 9.10d;
@@ -147,6 +163,8 @@ internal static class TrajectoryObservationChecks
configured.MaximumCurvaturePerMeter = 9.40d;
configured.SolverTimeoutSeconds = 9.50d;
configured.MaximumOsqpIterations = 9500;
configured.TimeHorizonSeconds = 5d;
configured.OutputTimeStepSeconds = 0.25d;
Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters,
"observer settings snapshot freezes vehicle length");
@@ -160,6 +178,10 @@ internal static class TrajectoryObservationChecks
"observer settings snapshot freezes solver timeout");
Verification.Equal(9000, snapshot.MaximumOsqpIterations,
"observer settings snapshot freezes OSQP iteration limit");
Verification.NearlyEqual(4d, snapshot.TimeHorizonSeconds,
"observer settings snapshot freezes ST time horizon");
Verification.NearlyEqual(0.20d, snapshot.OutputTimeStepSeconds,
"observer settings snapshot freezes ST timestamp spacing");
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot,
@@ -183,6 +205,13 @@ internal static class TrajectoryObservationChecks
AssertInvalidSetting(settings => settings.SolverTimeoutSeconds = 0d, "solver timeout");
AssertInvalidSetting(settings => settings.MaximumOsqpIterations = 0, "OSQP iteration limit");
AssertInvalidSetting(settings => settings.MaximumOsqpIterations = -1, "negative OSQP iteration limit");
AssertInvalidSetting(settings => settings.TimeHorizonSeconds = 0d, "ST time horizon");
AssertInvalidSetting(settings => settings.OutputTimeStepSeconds = 0d, "ST timestamp spacing");
AssertInvalidSetting(settings =>
{
settings.TimeHorizonSeconds = 1d;
settings.OutputTimeStepSeconds = 1.01d;
}, "ST timestamp spacing larger than the horizon");
}
private static void AssertInvalidSetting(Action<TrajectoryObservationSettings> mutate, string name)
@@ -404,7 +433,12 @@ internal static class TrajectoryObservationChecks
Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");
var settings = new TrajectoryObservationSettings { MaximumOsqpIterations = 9000 };
var settings = new TrajectoryObservationSettings
{
MaximumOsqpIterations = 9000,
TimeHorizonSeconds = 4d,
OutputTimeStepSeconds = 0.20d,
};
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
Array.Empty<TrajectoryObservationObstacle>(), 0L);
@@ -431,6 +465,12 @@ internal static class TrajectoryObservationChecks
Verification.Equal(settings.MaximumOsqpIterations,
planningService.Requests[0].Configuration.Solver.MaximumOsqpIterations,
"observer configured OSQP iteration limit");
Verification.NearlyEqual(settings.TimeHorizonSeconds,
planningService.Requests[0].Configuration.Scheduling.TimeHorizonSeconds,
"observer configured ST time horizon");
Verification.NearlyEqual(settings.OutputTimeStepSeconds,
planningService.Requests[0].Configuration.Scheduling.OutputTimeStepSeconds,
"observer configured ST timestamp spacing");
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,