fix: freeze observation planning snapshots

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<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,
+            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
-            segmentIndex, coordinator.PublishedTrajectory, now, now,
+            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

@@ -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");
+        EmPlanningRequest rollingRequest = planningService.Requests[1];
-        Verification.Equal(trajectory.Metadata.TrajectoryId, planningService.Requests[1].PreviousTrajectoryId,
+        Verification.True(ReferenceEquals(published, rollingRequest.PreviousTrajectory),
-            "observer rolls published trajectory identity into next request");
+            "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()