diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index df63806..07a0c6e 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -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 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(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); } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 060076c..4d0e218 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -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(), 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(), 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()