diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index 93ac075..935c432 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -189,8 +189,11 @@ public sealed class TrajectoryObservationController { private readonly TrajectoryObservationBootstrapResult bootstrap; private readonly EmPlannerConfiguration configuration; - private readonly EmPlanningCoordinator coordinator; - private readonly TrajectoryExecutor executor; + private readonly IEmPlanningService planningService; + private readonly TrajectoryObservationSegmentTracker segmentTracker; + private EmPlanningCoordinator coordinator; + private TrajectoryExecutor executor; + private EmTrajectory previousTrajectoryForVisualization; private readonly string sessionId; private long cycleId; @@ -211,13 +214,27 @@ public sealed class TrajectoryObservationController configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations; configuration.Scheduling.TimeHorizonSeconds = settings.TimeHorizonSeconds; configuration.Scheduling.OutputTimeStepSeconds = settings.OutputTimeStepSeconds; + this.planningService = planningService; coordinator = new EmPlanningCoordinator(planningService); executor = new TrajectoryExecutor(configuration); + segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, settings, + configuration.Longitudinal.StopSpeedToleranceMetersPerSecond); this.sessionId = sessionId; } public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory; + public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex]; + + public TrajectoryObservationSegmentState SegmentState => segmentTracker.State; + + public EmTrajectory PreviousTrajectoryForVisualization => previousTrajectoryForVisualization; + + internal EmPlannerConfiguration CreateEffectiveConfigurationSnapshot() + { + return configuration.Copy(); + } + public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic() { return TrajectoryObservationDiagnostics.CreateConfiguration(configuration); @@ -233,18 +250,31 @@ public sealed class TrajectoryObservationController { if (state == null) throw new ArgumentNullException(nameof(state)); - const int segmentIndex = 0; long currentCycleId = Interlocked.Increment(ref cycleId); EmTrajectory previousTrajectory = coordinator.PublishedTrajectory; var request = new EmPlanningRequest( bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration, - segmentIndex, previousTrajectory, now, now, + ActiveSegment.SegmentIndex, previousTrajectory, now, now, sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference", previousTrajectory?.Metadata.TrajectoryId ?? string.Empty, EmMotionModel.NonholonomicForwardReverse); return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken); } + public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state) + { + if (state == null) throw new ArgumentNullException(nameof(state)); + + TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, coordinator.PublishedTrajectory); + if (!update.Advanced) + return false; + + previousTrajectoryForVisualization = coordinator.PublishedTrajectory; + coordinator = new EmPlanningCoordinator(planningService); + executor = new TrajectoryExecutor(configuration); + return true; + } + public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state) { if (state == null) throw new ArgumentNullException(nameof(state)); @@ -253,8 +283,15 @@ public sealed class TrajectoryObservationController if (trajectory == null) return new TrajectoryObservationObservation(now, state, null, null, null, null); + TrajectoryObservationSegmentState segmentState = segmentTracker.State; + TravelDirection currentDirection = ActiveSegment.Direction; + bool waitingForDirection = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop || + segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection; + TravelDirection desiredDirection = waitingForDirection && segmentState.ExpectedDirection.HasValue + ? segmentState.ExpectedDirection.Value + : currentDirection; TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory, - trajectory.Metadata.Direction, trajectory.Metadata.Direction, true); + desiredDirection, currentDirection, !waitingForDirection); TrajectoryExecutionState executorState = executor.State; return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint, command, executorState); @@ -265,7 +302,8 @@ public sealed class TrajectoryObservationLoopTick { internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation, PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight, - bool planningStarted, bool planningCompleted) + bool planningStarted, bool planningCompleted, bool segmentAdvanced, + TrajectoryObservationSegmentState segmentState) { Observation = observation ?? throw new ArgumentNullException(nameof(observation)); LatestCycle = latestCycle; @@ -273,6 +311,8 @@ public sealed class TrajectoryObservationLoopTick PlanningInFlight = planningInFlight; PlanningStarted = planningStarted; PlanningCompleted = planningCompleted; + SegmentAdvanced = segmentAdvanced; + SegmentState = segmentState ?? throw new ArgumentNullException(nameof(segmentState)); } public TrajectoryObservationObservation Observation { get; } @@ -287,6 +327,10 @@ public sealed class TrajectoryObservationLoopTick public bool PlanningCompleted { get; } + public bool SegmentAdvanced { get; } + + public TrajectoryObservationSegmentState SegmentState { get; } + public bool ShouldLog => true; } @@ -310,6 +354,9 @@ public sealed class TrajectoryObservationLoop cancellationToken.ThrowIfCancellationRequested(); bool planningCompleted = ConsumeCompletedPlanning(now); + bool segmentAdvanced = planningTask == null && controller.TryAdvanceSegment(now, state); + if (segmentAdvanced) + latestCycle = null; bool planningStarted = false; if (planningTask == null && controller.ShouldStartCycle(now)) { @@ -321,7 +368,7 @@ public sealed class TrajectoryObservationLoop TrajectoryObservationObservation observation = controller.Observe(now, state); return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed, - planningTask != null, planningStarted, planningCompleted); + planningTask != null, planningStarted, planningCompleted, segmentAdvanced, controller.SegmentState); } private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index eb8e094..a681d38 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -38,6 +38,8 @@ internal static class TrajectoryObservationChecks VerifiesEmptyChartsReceivePersistentPlanningDiagnostic(); VerifiesLsPresentationUsesPathSOnHorizontalAxis(); VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot(); + VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories(); + VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed(); FreezesBootstrapVehicleForRollingRequests(); VerifiesEmptyMapBootstrapProducesPublishableReference(); } @@ -543,6 +545,125 @@ internal static class TrajectoryObservationChecks "observer rolling request trajectory object and ID use one publication snapshot"); } + private static void VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories() + { + DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 4, 0, 0, TimeSpan.Zero); + var settings = new TrajectoryObservationSettings + { + DirectionConfirmationSamples = 1, + }; + CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( + new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, + Array.Empty(), 0L); + TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper() + .Bootstrap(job, CancellationToken.None); + Verification.True(baseBootstrap.Succeeded, "active-segment controller bootstrap succeeds"); + + TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success( + baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments()); + EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "active-forward", 0, + TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d); + EmTrajectory reverseTrajectory = CreateSegmentTrajectory(t0.AddSeconds(1d), "active-reverse", 1, + TravelDirection.Reverse, EmTerminalType.Goal, EmBoundaryType.Goal, 0d); + var planningService = new SequenceTrajectoryPlanningService(forwardGearTrajectory, reverseTrajectory, + reverseTrajectory); + var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "active-segment"); + + VehicleMotionState stopped = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L); + controller.StartCycle(t0, stopped, CancellationToken.None).GetAwaiter().GetResult(); + Verification.Equal(0, planningService.Requests[0].SegmentIndex, + "first active-segment request uses segment zero"); + Verification.Equal(0, controller.ActiveSegment.SegmentIndex, "controller exposes initial active segment"); + + controller.TryAdvanceSegment(t0, stopped); + controller.TryAdvanceSegment(t0.AddSeconds(0.21d), new VehicleMotionState( + new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L)); + bool advanced = controller.TryAdvanceSegment(t0.AddSeconds(0.22d), new VehicleMotionState( + new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 3L)); + + Verification.True(advanced, "confirmed reverse direction advances the controller"); + Verification.Equal(1, controller.ActiveSegment.SegmentIndex, "controller uses tracker-confirmed segment"); + Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PreviousTrajectoryForVisualization), + "old direction trajectory is retained only for visualization"); + + DateTimeOffset reverseAt = t0.AddSeconds(1d); + controller.StartCycle(reverseAt, new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null, + reverseAt, 4L), CancellationToken.None).GetAwaiter().GetResult(); + Verification.Equal(1, planningService.Requests[1].SegmentIndex, + "next active-segment request uses segment one"); + Verification.True(planningService.Requests[1].PreviousTrajectory == null, + "new direction does not reuse the old trajectory as an EM seed"); + Verification.Equal("active-segment-trajectory-2", planningService.Requests[1].OutputTrajectoryId, + "session cycle IDs remain monotonic after coordinator replacement"); + + controller.StartCycle(reverseAt.AddSeconds(settings.ReplanPeriodSeconds), new VehicleMotionState( + new Pose2D(0.9d, 0d, 0d), -0.03d, null, reverseAt.AddSeconds(settings.ReplanPeriodSeconds), 5L), + CancellationToken.None).GetAwaiter().GetResult(); + Verification.True(ReferenceEquals(reverseTrajectory, planningService.Requests[2].PreviousTrajectory), + "same direction rolling request retains its published trajectory as the seed"); + } + + private static void VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed() + { + DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 5, 0, 0, TimeSpan.Zero); + var settings = new TrajectoryObservationSettings + { + DirectionConfirmationSamples = 1, + }; + CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( + new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, + Array.Empty(), 0L); + TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper() + .Bootstrap(job, CancellationToken.None); + TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success( + baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments()); + EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "loop-forward", 0, + TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d); + var planningService = new DelayedTrajectoryPlanningService(forwardGearTrajectory, 2); + try + { + var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "loop-segment"); + controller.StartCycle(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L), + CancellationToken.None).GetAwaiter().GetResult(); + var loop = new TrajectoryObservationLoop(controller); + + loop.Tick(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 2L), + CancellationToken.None); + TrajectoryObservationLoopTick planningTick = loop.Tick(t0.AddSeconds(0.21d), + new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 3L), + CancellationToken.None); + Verification.True(planningTick.PlanningInFlight, "new cycle is in flight before direction evidence"); + Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)), + "delayed cycle entered planning service"); + + TrajectoryObservationLoopTick blockedTick = loop.Tick(t0.AddSeconds(0.22d), + new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 4L), + CancellationToken.None); + Verification.True(!blockedTick.SegmentAdvanced, "in-flight planning prevents a segment transition"); + Verification.Equal(0, blockedTick.SegmentState.ActiveSegmentIndex, + "in-flight planning retains the immutable old segment state"); + + planningService.Release(); + TrajectoryObservationLoopTick? advancedTick = null; + int completedSequence = 5; + bool advanced = SpinWait.SpinUntil(() => + { + DateTimeOffset now = t0.AddSeconds(0.23d + (completedSequence - 5) * 0.01d); + advancedTick = loop.Tick(now, new VehicleMotionState( + new Pose2D(1d, 0d, 0d), -0.03d, null, now, completedSequence++), CancellationToken.None); + return advancedTick.SegmentAdvanced; + }, TimeSpan.FromSeconds(5d)); + Verification.True(advanced, + "loop advances only after it has consumed the completed planning task"); + Verification.Equal(1, advancedTick!.SegmentState.ActiveSegmentIndex, + "loop tick exposes the advanced immutable segment state"); + } + finally + { + planningService.Release(); + } + } + private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero); @@ -788,6 +909,47 @@ internal static class TrajectoryObservationChecks }); } + private static IReadOnlyList CreateDirectionalSegments() + { + return new[] + { + CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false, + EmBoundaryType.None, EmBoundaryType.GearSwitchApproach), + CreateDirectionalSegment(1, TravelDirection.Reverse, 1d, 0d, true, + EmBoundaryType.GearSwitchDeparture, EmBoundaryType.Goal), + }; + } + + private static DirectionSegmentView CreateDirectionalSegment(int index, TravelDirection direction, double startX, + double endX, bool startIsGearSwitch, EmBoundaryType startBoundary, EmBoundaryType endBoundary) + { + var points = new List + { + new SmoothedPathPoint(startX, 0d, 0d, 0d, 0d, direction, 0d, 0d, 1d, + startIsGearSwitch, SmoothedPathPointSource.Anchor), + new SmoothedPathPoint(endX, 0d, 0d, 0d, 1d, direction, 0d, 0d, 1d, + false, SmoothedPathPointSource.Anchor), + }; + return new DirectionSegmentView(index, direction, points, + new ReferenceBoundary(index, 0d, startBoundary, index), + new ReferenceBoundary(index, 1d, endBoundary, index + 1d), index); + } + + private static EmTrajectory CreateSegmentTrajectory(DateTimeOffset effectiveAt, string trajectoryId, int segmentIndex, + TravelDirection direction, EmTerminalType terminalType, EmBoundaryType boundaryType, double x) + { + var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 1L, + "active-segment-reference", 1L, string.Empty, segmentIndex, direction, terminalType, + terminalType == EmTerminalType.GearSwitch + ? EmLongitudinalMode.ExactStopAtBoundary + : EmLongitudinalMode.RollingContinuation); + return new EmTrajectory(metadata, new[] + { + new EmTrajectoryPoint(x, 0d, 0d, 0d, 0d, 0d, segmentIndex, 1d, 1d, + direction, boundaryType, 0d, 0d), + }); + } + private sealed class FixedTrajectoryPlanningService : IEmPlanningService { private readonly EmTrajectory trajectory; @@ -806,6 +968,27 @@ internal static class TrajectoryObservationChecks } } + private sealed class SequenceTrajectoryPlanningService : IEmPlanningService + { + private readonly IReadOnlyList trajectories; + private int nextTrajectoryIndex; + + public SequenceTrajectoryPlanningService(params EmTrajectory[] trajectories) + { + this.trajectories = trajectories ?? throw new ArgumentNullException(nameof(trajectories)); + } + + public List Requests { get; } = new List(); + + public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) + { + Requests.Add(request); + EmTrajectory trajectory = trajectories[Math.Min(nextTrajectoryIndex, trajectories.Count - 1)]; + nextTrajectoryIndex++; + return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty); + } + } + private sealed class DelayedTrajectoryPlanningService : IEmPlanningService { private readonly EmTrajectory trajectory; diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs index d578638..c2bfd2a 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.IO; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.EMPlanner; using MultiWheelC.TrajectoryPlanning.PathSmoothing; @@ -11,12 +12,23 @@ internal static class TrajectoryObservationSegmentChecks { public static void Run() { + RejectsHardcodedActiveSegmentIndex(); AdvancesOnlyAfterContinuousStopAndStableNextDirection(); RejectsNonTerminalOrMismatchedGearTrajectory(); ResetsConfirmationForInvalidDirectionEvidence(); CompletesOneSegmentWithoutIndexingPastTheEnd(); } + private static void RejectsHardcodedActiveSegmentIndex() + { + string pipelinePath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", + "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPipeline.cs"); + string source = File.ReadAllText(pipelinePath); + + Verification.True(source.IndexOf("segmentIndex = 0", StringComparison.Ordinal) < 0, + "active observation segment is not hardcoded to zero"); + } + private static void AdvancesOnlyAfterContinuousStopAndStableNextDirection() { DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 0, 0, 0, TimeSpan.Zero);