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

feat: visualize EM observation diagnostics

Browse Source
This commit is contained in:
梁薄云
2026-08-04 16:21:17 +08:00
parent 3a23fc2fe3
commit 08f6603cad
2 changed files with 299 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
+32
View File
@@ -17,6 +17,7 @@ internal static class TrajectoryObservationChecks
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
RejectsObstacleOutsideConfiguredBounds();
VerifiesLsAndStUsePublishedTrajectoryData();
VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
FreezesBootstrapVehicleForRollingRequests();
}
@@ -123,6 +124,37 @@ internal static class TrajectoryObservationChecks
"observer rolling request trajectory object and ID use one publication snapshot");
}
private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
{
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
var settings = new TrajectoryObservationSettings();
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);
var controller = new TrajectoryObservationController(bootstrap, settings,
new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check");
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L);
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
observation.PublishedTrajectory, CreateStraightSegment(), 0.5d);
string text = TrajectoryObservationPresentationText.Create(observation, charts);
Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."),
"observer presentation observe-only notice");
Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"),
"observer presentation selected point time and path-S");
Verification.True(text.Contains("signed speed=0.30 m/s"),
"observer presentation signed speed");
Verification.True(text.Contains("yaw rate=0.00 rad/s"),
"observer presentation yaw rate");
Verification.True(text.Contains("LS projection failures=0"),
"observer presentation LS projection failures");
}
private static void FreezesBootstrapVehicleForRollingRequests()
{
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);