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/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs
+22 -3
View File
@@ -14,10 +14,13 @@ namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
public sealed class TrajectoryObservationBootstrapResult
{
private readonly VehicleParameters vehicle;
private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
{
Job = job ?? throw new ArgumentNullException(nameof(job));
vehicle = CopyVehicle(job.Vehicle);
CoarseResult = coarse;
SmoothedPath = smoothedPath;
Segments = CopySegments(segments);
@@ -29,6 +32,8 @@ public sealed class TrajectoryObservationBootstrapResult
public CoarsePathPlanningJob Job { get; }
public VehicleParameters Vehicle => CopyVehicle(vehicle);
public CoarsePathPlanningJobResult CoarseResult { get; }
public PathSmoothingResult SmoothedPath { get; }
@@ -67,6 +72,19 @@ public sealed class TrajectoryObservationBootstrapResult
}
return new ReadOnlyCollection<DirectionSegmentView>(copy);
}
private static VehicleParameters CopyVehicle(VehicleParameters source)
{
if (source == null) return null;
return new VehicleParameters
{
LengthMeters = source.LengthMeters,
WidthMeters = source.WidthMeters,
SafetyMarginMeters = source.SafetyMarginMeters,
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
};
}
}
public sealed class TrajectoryObservationBootstrapper
@@ -208,11 +226,12 @@ public sealed class TrajectoryObservationController
const int segmentIndex = 0;
long currentCycleId = Interlocked.Increment(ref cycleId);
EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
var request = new EmPlanningRequest(
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Job.Vehicle, state, configuration,
segmentIndex, coordinator.PublishedTrajectory, now, now,
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
segmentIndex, previousTrajectory, now, now,
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
coordinator.PublishedTrajectory?.Metadata.TrajectoryId ?? string.Empty,
previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
EmMotionModel.NonholonomicForwardReverse);
return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
}
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()