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

fix: freeze observation planning snapshots

Browse Source
This commit is contained in:
梁薄云
2026-08-04 16:13:09 +08:00
parent 0f5255c598
commit 3a23fc2fe3
2 changed files with 107 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
+85 -4
View File
@@ -17,6 +17,8 @@ internal static class TrajectoryObservationChecks
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
RejectsObstacleOutsideConfiguredBounds();
VerifiesLsAndStUsePublishedTrajectoryData();
VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
FreezesBootstrapVehicleForRollingRequests();
}
private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
@@ -92,13 +94,92 @@ internal static class TrajectoryObservationChecks
TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
"observer executor interpolates from published effective time");
}
private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot()
{
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 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);
Verification.True(bootstrap.Succeeded, "observer rolling snapshot bootstrap succeeds");
EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
var planningService = new FixedTrajectoryPlanningService(published);
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "snapshot-check");
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 2L);
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
.GetAwaiter().GetResult();
Verification.Equal(trajectory, planningService.Requests[1].PreviousTrajectory,
"observer rolls published trajectory into next request");
Verification.Equal(trajectory.Metadata.TrajectoryId, planningService.Requests[1].PreviousTrajectoryId,
"observer rolls published trajectory identity into next request");
EmPlanningRequest rollingRequest = planningService.Requests[1];
Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory),
"observer rolling request uses the controlled published trajectory object");
Verification.Equal(rollingRequest.PreviousTrajectory.Metadata.TrajectoryId,
rollingRequest.PreviousTrajectoryId,
"observer rolling request trajectory object and ID use one publication snapshot");
}
private static void FreezesBootstrapVehicleForRollingRequests()
{
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 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);
VehicleParameters originalVehicle = job.Vehicle;
double expectedLength = originalVehicle.LengthMeters;
double expectedWidth = originalVehicle.WidthMeters;
double expectedMargin = originalVehicle.SafetyMarginMeters;
double? expectedMaximumCurvature = originalVehicle.MaximumCurvaturePerMeter;
double? expectedMinimumRadius = originalVehicle.MinimumTurningRadiusMeters;
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
.Bootstrap(job, CancellationToken.None);
Verification.True(bootstrap.Succeeded, "observer vehicle snapshot bootstrap succeeds");
originalVehicle.LengthMeters = 91d;
originalVehicle.WidthMeters = 92d;
originalVehicle.SafetyMarginMeters = 93d;
originalVehicle.MaximumCurvaturePerMeter = 94d;
originalVehicle.MinimumTurningRadiusMeters = 95d;
EmTrajectory published = CreatePublishedTrajectory(effectiveAt);
var planningService = new FixedTrajectoryPlanningService(published);
var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "vehicle-check");
var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 3L);
controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
AssertVehicleSnapshot(planningService.Requests[0].Vehicle, expectedLength, expectedWidth, expectedMargin,
expectedMaximumCurvature, expectedMinimumRadius, "mutated bootstrap job vehicle");
Verification.True(!ReferenceEquals(originalVehicle, planningService.Requests[0].Vehicle),
"observer request does not retain mutable bootstrap vehicle object");
job.Vehicle = new VehicleParameters
{
LengthMeters = 101d,
WidthMeters = 102d,
SafetyMarginMeters = 103d,
MaximumCurvaturePerMeter = 104d,
MinimumTurningRadiusMeters = 105d,
};
controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
.GetAwaiter().GetResult();
AssertVehicleSnapshot(planningService.Requests[1].Vehicle, expectedLength, expectedWidth, expectedMargin,
expectedMaximumCurvature, expectedMinimumRadius, "replaced bootstrap job vehicle");
}
private static void AssertVehicleSnapshot(VehicleParameters actual, double expectedLength, double expectedWidth,
double expectedMargin, double? expectedMaximumCurvature, double? expectedMinimumRadius, string name)
{
Verification.NearlyEqual(expectedLength, actual.LengthMeters, name + " length");
Verification.NearlyEqual(expectedWidth, actual.WidthMeters, name + " width");
Verification.NearlyEqual(expectedMargin, actual.SafetyMarginMeters, name + " safety margin");
Verification.Equal(expectedMaximumCurvature, actual.MaximumCurvaturePerMeter,
name + " maximum curvature");
Verification.Equal(expectedMinimumRadius, actual.MinimumTurningRadiusMeters,
name + " minimum turning radius");
}
private static DirectionSegmentView CreateStraightSegment()