From ab3204bfde18cd2709fc4684d01fee9e622287da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 08:56:14 +0800 Subject: [PATCH] feat: observe one full EM direction segment --- .../MovementTest.TrajectoryObservationTest.cs | 10 +- .../TrajectoryObservationContracts.cs | 9 +- .../TrajectoryObservationPipeline.cs | 33 ++- ...jectoryObservationStaticSnapshotBuilder.cs | 1 + .../TrajectoryObservationChecks.cs | 230 +++++++++++++++++- .../TrajectoryObservationSettingsChecks.cs | 26 +- 6 files changed, 289 insertions(+), 20 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index 142bb77..e77fb89 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -32,22 +32,23 @@ public sealed class TrajectoryObservationMovementTest : MovementTest public float MapResolutionMm = 50f; public double ReplanPeriodSeconds = 0.20d; public double ObserverPeriodSeconds = 0.05d; + public bool UseFullDirectionSegmentPlanning = true; public double SolverTimeoutSeconds = 5.0d; public int MaximumOsqpIterations = 100000; - /// 单次 ST 轨迹覆盖的未来时长，单位 s；默认 2s，不等于观察循环周期。 - public double TimeHorizonSeconds = 2d; + /// 仅滚动兼容字段；完整方向段模式不用于截断。默认 2s，不等于观察循环周期。 + public double TimeHorizonSeconds = 20d; /// Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。 public double OutputTimeStepSeconds = 0.10d; public double VehicleLengthMeters = 0.80d; public double VehicleWidthMeters = 0.60d; public double SafetyMarginMeters = 0.05d; public double MaximumCurvaturePerMeter = 1d / 1.20d; - public bool EnableWebVisualization = false; + public bool EnableWebVisualization = true; public bool AutoOpenWebVisualization = true; public int WebVisualizationPort = 0; public double WebRefreshRateHz = 10d; public int VisualizationHistoryCycleLimit = 60; - public bool EnableNativePainterVisualization = false; + public bool EnableNativePainterVisualization = true; public double DirectionConfirmationSpeedMetersPerSecond = 0.02d; public int DirectionConfirmationSamples = 3; public double GearSwitchProjectionToleranceMeters = 0.50d; @@ -76,6 +77,7 @@ public sealed class TrajectoryObservationMovementTest : MovementTest MapResolutionMillimeters = MapResolutionMm, ReplanPeriodSeconds = ReplanPeriodSeconds, ObserverPeriodSeconds = ObserverPeriodSeconds, + PlanningScope = UseFullDirectionSegmentPlanning ? EmPlanningScope.FullDirectionSegment : EmPlanningScope.RollingHorizon, SolverTimeoutSeconds = SolverTimeoutSeconds, MaximumOsqpIterations = MaximumOsqpIterations, TimeHorizonSeconds = TimeHorizonSeconds, diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs index 2436512..c84d380 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs @@ -2,19 +2,21 @@ using System; using System.Collections.Generic; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade; +using MultiWheelC.TrajectoryPlanning.EMPlanner; using MultiWheelC.TrajectoryPlanning.Mapping; namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; public sealed class TrajectoryObservationSettings { + public EmPlanningScope PlanningScope { get; set; } = EmPlanningScope.FullDirectionSegment; public double MapPaddingMeters { get; set; } = 2d; public float MapResolutionMillimeters { get; set; } = 50f; public double ReplanPeriodSeconds { get; set; } = 0.20d; public double ObserverPeriodSeconds { get; set; } = 0.05d; public double SolverTimeoutSeconds { get; set; } = 5d; public int MaximumOsqpIterations { get; set; } = 100000; - /// 单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。 + /// 仅滚动视窗兼容字段；完整方向段模式不用于截断。单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。 public double TimeHorizonSeconds { get; set; } = 2d; /// 发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。 public double OutputTimeStepSeconds { get; set; } = 0.10d; @@ -22,7 +24,7 @@ public sealed class TrajectoryObservationSettings public double VehicleWidthMeters { get; set; } = 0.60d; public double SafetyMarginMeters { get; set; } = 0.05d; public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d; - public bool EnableWebVisualization { get; set; } = false; + public bool EnableWebVisualization { get; set; } = true; public bool AutoOpenWebVisualization { get; set; } = true; public int WebVisualizationPort { get; set; } = 0; public double WebRefreshRateHz { get; set; } = 10d; @@ -37,6 +39,7 @@ public sealed class TrajectoryObservationSettings { var snapshot = new TrajectoryObservationSettings { + PlanningScope = PlanningScope, MapPaddingMeters = MapPaddingMeters, MapResolutionMillimeters = MapResolutionMillimeters, ReplanPeriodSeconds = ReplanPeriodSeconds, @@ -66,6 +69,8 @@ public sealed class TrajectoryObservationSettings public void Validate() { + if (!Enum.IsDefined(typeof(EmPlanningScope), PlanningScope)) + throw new ArgumentOutOfRangeException(nameof(PlanningScope), "Value must be a defined planning scope."); EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters)); EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters)); EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds)); diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index 0b73a31..f5fbd8a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -188,6 +188,7 @@ public sealed class TrajectoryObservationObservation public sealed class TrajectoryObservationController { private readonly TrajectoryObservationBootstrapResult bootstrap; + private readonly TrajectoryObservationSettings settings; private readonly EmPlannerConfiguration configuration; private readonly IEmPlanningService planningService; private readonly TrajectoryObservationSegmentTracker segmentTracker; @@ -196,6 +197,8 @@ public sealed class TrajectoryObservationController private EmTrajectory previousTrajectoryForVisualization; private readonly string sessionId; private long cycleId; + private int plannedSegmentIndex; + private bool planAttemptedForActiveSegment; public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId) @@ -207,17 +210,18 @@ public sealed class TrajectoryObservationController if (planningService == null) throw new ArgumentNullException(nameof(planningService)); if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId)); - settings.Validate(); + TrajectoryObservationSettings snapshot = settings.CreateValidatedSnapshot(); + this.settings = snapshot; configuration = EmPlannerConfiguration.CreateDefault(); - configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds; - configuration.Scheduling.SolverTimeoutSeconds = settings.SolverTimeoutSeconds; - configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations; - configuration.Scheduling.TimeHorizonSeconds = settings.TimeHorizonSeconds; - configuration.Scheduling.OutputTimeStepSeconds = settings.OutputTimeStepSeconds; + configuration.Scheduling.ReplanPeriodSeconds = snapshot.ReplanPeriodSeconds; + configuration.Scheduling.SolverTimeoutSeconds = snapshot.SolverTimeoutSeconds; + configuration.Solver.MaximumOsqpIterations = snapshot.MaximumOsqpIterations; + configuration.Scheduling.TimeHorizonSeconds = snapshot.TimeHorizonSeconds; + configuration.Scheduling.OutputTimeStepSeconds = snapshot.OutputTimeStepSeconds; this.planningService = planningService; coordinator = new EmPlanningCoordinator(planningService); executor = new TrajectoryExecutor(configuration); - segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, settings, + segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, snapshot, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond); this.sessionId = sessionId; } @@ -242,6 +246,15 @@ public sealed class TrajectoryObservationController public bool ShouldStartCycle(DateTimeOffset now) { + if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + if (plannedSegmentIndex != ActiveSegment.SegmentIndex) + { + plannedSegmentIndex = ActiveSegment.SegmentIndex; + planAttemptedForActiveSegment = false; + } + return !planAttemptedForActiveSegment; + } return coordinator.ShouldStartCycle(now); } @@ -250,6 +263,8 @@ public sealed class TrajectoryObservationController { if (state == null) throw new ArgumentNullException(nameof(state)); + planAttemptedForActiveSegment = true; + plannedSegmentIndex = ActiveSegment.SegmentIndex; long currentCycleId = Interlocked.Increment(ref cycleId); EmTrajectory previousTrajectory = coordinator.PublishedTrajectory; var request = new EmPlanningRequest( @@ -257,7 +272,7 @@ public sealed class TrajectoryObservationController ActiveSegment.SegmentIndex, previousTrajectory, now, now, sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference", previousTrajectory?.Metadata.TrajectoryId ?? string.Empty, - EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon); + EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope); return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken); } @@ -272,6 +287,8 @@ public sealed class TrajectoryObservationController previousTrajectoryForVisualization = coordinator.PublishedTrajectory; coordinator = new EmPlanningCoordinator(planningService); executor = new TrajectoryExecutor(configuration); + plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex; + planAttemptedForActiveSegment = false; return true; } diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs index 4f3e2f4..4915a0e 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs @@ -168,6 +168,7 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder V("行数", "Rows", I(map.Rows), ""), V("列数", "Cols", I(map.Cols), "")), Group("可视化与换向确认", + V("规划范围", "PlanningScope", settings.PlanningScope.ToString(), ""), V("启用网页", "EnableWebVisualization", B(settings.EnableWebVisualization), ""), V("自动打开网页", "AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization), ""), V("网页端口", "WebVisualizationPort", I(settings.WebVisualizationPort), ""), diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 11e1b0d..fae1757 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -1,6 +1,9 @@ using System; using System.Collections.Generic; +using System.Globalization; using System.IO; +using System.Linq; +using System.Reflection; using System.Text; using System.Threading; using MultiWheelC.TrajectoryPlanning.CoarsePath; @@ -10,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.Mapping; using MultiWheelC.TrajectoryPlanning.PathSmoothing; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade; using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; +using TrajectoryPlanningVisualization; namespace EMPlannerVerificationHost; @@ -31,6 +35,11 @@ internal static class TrajectoryObservationChecks VerifiesValidNonEmptyObstacleSource(); RejectsObstacleOutsideConfiguredBounds(); VerifiesObserverTicksWhilePlanningIsDelayed(); + VerifiesFullDirectionSegmentPlansOncePerActiveSegment(); + VerifiesFullDirectionRequestCarriesValidatedScope(); + VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition(); + VerifiesFailedFullPlanDoesNotAutoRetry(); + VerifiesStaticSnapshotExportsPlanningScope(); VerifiesSessionLayerCleanupDecisions(); VerifiesGearSwitchWaitStateForWorldPresentation(); VerifiesLsAndStUsePublishedTrajectoryData(); @@ -213,9 +222,15 @@ internal static class TrajectoryObservationChecks new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic"); string text = controller.CreateConfigurationDiagnostic().Text; + MethodInfo effectiveConfigurationMethod = typeof(TrajectoryObservationController).GetMethod( + "CreateEffectiveConfigurationSnapshot", BindingFlags.Instance | BindingFlags.NonPublic)!; + var effectiveConfiguration = (EmPlannerConfiguration)effectiveConfigurationMethod.Invoke( + controller, Array.Empty())!; + string distanceHorizonText = "distanceHorizon=" + + effectiveConfiguration.Scheduling.DistanceHorizonMeters.ToString("F2", CultureInfo.InvariantCulture) + "m"; foreach (string expected in new[] { - "planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s", + "planning configuration:", "timeHorizon=3.50s", distanceHorizonText, "outputTimeStep=0.10s", "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321", "solverTimeout=1.25s", "replanPeriod=0.25s", "maximumJerkLimitedStopDistance=", "maximumJerkLimitedStopDuration=", "requiredDistanceHorizon=", @@ -316,7 +331,10 @@ internal static class TrajectoryObservationChecks private static void VerifiesObserverTicksWhilePlanningIsDelayed() { DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 4, 3, 0, 0, TimeSpan.Zero); - var settings = new TrajectoryObservationSettings(); + var settings = new TrajectoryObservationSettings + { + PlanningScope = EmPlanningScope.RollingHorizon, + }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); @@ -412,6 +430,186 @@ internal static class TrajectoryObservationChecks } } + private static void VerifiesFullDirectionSegmentPlansOncePerActiveSegment() + { + DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 7, 0, 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, "one-shot bootstrap succeeds"); + + EmTrajectory published = CreatePublishedTrajectory(startedAt); + var planningService = new FixedTrajectoryPlanningService(published); + var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "one-shot"); + var loop = new TrajectoryObservationLoop(controller); + controller.StartCycle(startedAt, + new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 1L), + CancellationToken.None).GetAwaiter().GetResult(); + + TrajectoryObservationLoopTick? lastTick = null; + for (int index = 0; index < 20; index++) + { + DateTimeOffset now = startedAt.AddSeconds((index + 1) * settings.ObserverPeriodSeconds); + lastTick = loop.Tick(now, + new VehicleMotionState(new Pose2D(0.10d + index * 0.01d, 0d, 0d), 0.10d, null, + now, 2L + index), CancellationToken.None); + } + + Verification.Equal(1, planningService.Requests.Count, + "full direction segment is planned only once per active segment"); + Verification.True(lastTick != null && !lastTick.PlanningStarted, + "no coordinator-cadence replan after the full segment plan"); + Verification.True(lastTick != null && lastTick.Observation != null && + ReferenceEquals(published, lastTick.Observation.PublishedTrajectory), + "observation continues to publish the frozen full-segment trajectory"); + } + + private static void VerifiesFullDirectionRequestCarriesValidatedScope() + { + DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 7, 1, 0, 0, TimeSpan.Zero); + var source = new TrajectoryObservationSettings + { + PlanningScope = EmPlanningScope.FullDirectionSegment, + }; + TrajectoryObservationSettings snapshot = source.CreateValidatedSnapshot(); + source.PlanningScope = EmPlanningScope.RollingHorizon; + + CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( + new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot, + Array.Empty(), 0L); + TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper() + .Bootstrap(job, CancellationToken.None); + Verification.True(bootstrap.Succeeded, "scope propagation bootstrap succeeds"); + + EmTrajectory published = CreatePublishedTrajectory(effectiveAt); + var planningService = new FixedTrajectoryPlanningService(published); + var controller = new TrajectoryObservationController(bootstrap, snapshot, planningService, "scope-check"); + snapshot.PlanningScope = EmPlanningScope.RollingHorizon; + var state = new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, effectiveAt, 1L); + controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult(); + + Verification.Equal(EmPlanningScope.FullDirectionSegment, planningService.Requests[0].PlanningScope, + "controller propagates the validated full-direction scope into EM requests"); + Verification.True(!controller.ShouldStartCycle(effectiveAt.AddSeconds(1d)), + "full scope blocks coordinator-cadence restart after the first plan"); + } + + private static void VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition() + { + DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 2, 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, "one-shot transition bootstrap succeeds"); + TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success( + baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments()); + + EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "one-shot-forward", 0, + TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d); + EmTrajectory reverseTrajectory = CreateSegmentTrajectory(t0.AddSeconds(1d), "one-shot-reverse", 1, + TravelDirection.Reverse, EmTerminalType.Goal, EmBoundaryType.Goal, 0d); + var planningService = new SequenceTrajectoryPlanningService(forwardGearTrajectory, reverseTrajectory, + reverseTrajectory); + var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "one-shot-advance"); + var stoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L); + controller.StartCycle(t0, stoppedState, CancellationToken.None).GetAwaiter().GetResult(); + + Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)), + "no replan is armed before a confirmed segment transition"); + controller.TryAdvanceSegment(t0, stoppedState); + controller.TryAdvanceSegment(t0.AddSeconds(0.21d), + new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L)); + Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)), + "stop hold and direction waiting do not reset the one-shot flag"); + + var reverseState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null, + t0.AddSeconds(0.22d), 3L); + Verification.True(controller.TryAdvanceSegment(t0.AddSeconds(0.22d), reverseState), + "confirmed N to N+1 transition succeeds"); + Verification.True(controller.ShouldStartCycle(t0.AddSeconds(0.23d)), + "one-shot planning is rearmed only after the confirmed transition"); + + controller.StartCycle(t0.AddSeconds(0.23d), reverseState, CancellationToken.None).GetAwaiter().GetResult(); + Verification.Equal(2, planningService.Requests.Count, + "the next active direction segment receives exactly one new plan"); + Verification.Equal(1, planningService.Requests[1].SegmentIndex, + "the new full-direction plan targets segment N+1"); + Verification.Equal(EmPlanningScope.FullDirectionSegment, planningService.Requests[1].PlanningScope, + "the new full-direction plan keeps the full scope"); + } + + private static void VerifiesFailedFullPlanDoesNotAutoRetry() + { + DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 3, 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, "failed-plan bootstrap succeeds"); + + var planningService = new FailingTrajectoryPlanningService(); + var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "failed-one-shot"); + var loop = new TrajectoryObservationLoop(controller); + var state = new VehicleMotionState(new Pose2D(0.10d, 0d, 0d), 0.10d, null, t0, 1L); + loop.Tick(t0, state, CancellationToken.None); + + TrajectoryObservationLoopTick? failureTick = null; + bool consumed = SpinWait.SpinUntil(() => + { + DateTimeOffset now = t0.AddSeconds(0.05d); + var current = new VehicleMotionState(new Pose2D(0.11d, 0d, 0d), 0.10d, null, now, 2L); + failureTick = loop.Tick(now, current, CancellationToken.None); + return failureTick.LatestCycle != null; + }, TimeSpan.FromSeconds(5d)); + Verification.True(consumed, "failed full plan is consumed and visible"); + Verification.Equal(1, planningService.Requests.Count, + "failed full plan makes exactly one attempt"); + Verification.True(failureTick != null && failureTick.LatestCycle != null && + !failureTick.LatestCycle.Published, + "failed full plan remains visible as unpublished"); + + TrajectoryObservationLoopTick? lastTick = null; + for (int index = 0; index < 20; index++) + { + DateTimeOffset now = t0.AddSeconds(0.10d + (index + 1) * settings.ObserverPeriodSeconds); + lastTick = loop.Tick(now, + new VehicleMotionState(new Pose2D(0.10d + index * 0.01d, 0d, 0d), 0.10d, null, + now, 3L + index), CancellationToken.None); + } + Verification.Equal(1, planningService.Requests.Count, + "failed full plan does not automatically roll into coordinator-cadence retries"); + Verification.True(lastTick != null && lastTick.Observation != null && !lastTick.PlanningStarted, + "observation continues after a failed full plan without starting retries"); + } + + private static void VerifiesStaticSnapshotExportsPlanningScope() + { + 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, "static scope snapshot bootstrap succeeds"); + + PlanningVisualizationStaticSnapshot snapshot = new TrajectoryObservationStaticSnapshotBuilder().Build( + bootstrap, EmPlannerConfiguration.CreateDefault(), settings.CreateValidatedSnapshot(), 0L); + Verification.True(snapshot.ConfigurationGroups.SelectMany(x => x.Entries).Any(x => + x.RawName == "PlanningScope" && x.Value == "FullDirectionSegment"), + "static snapshot exports the validated full-direction scope"); + } + private static void VerifiesSessionLayerCleanupDecisions() { Verification.True(!TrajectoryObservationSessionLifecycle.ShouldClearLayers( @@ -523,7 +721,10 @@ internal static class TrajectoryObservationChecks private static void VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 1, 0, 0, TimeSpan.Zero); - var settings = new TrajectoryObservationSettings(); + var settings = new TrajectoryObservationSettings + { + PlanningScope = EmPlanningScope.RollingHorizon, + }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); @@ -610,6 +811,7 @@ internal static class TrajectoryObservationChecks DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 5, 0, 0, TimeSpan.Zero); var settings = new TrajectoryObservationSettings { + PlanningScope = EmPlanningScope.RollingHorizon, DirectionConfirmationSamples = 1, }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( @@ -669,7 +871,10 @@ internal static class TrajectoryObservationChecks private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero); - var settings = new TrajectoryObservationSettings(); + var settings = new TrajectoryObservationSettings + { + PlanningScope = EmPlanningScope.RollingHorizon, + }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); @@ -788,7 +993,10 @@ internal static class TrajectoryObservationChecks private static void FreezesBootstrapVehicleForRollingRequests() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero); - var settings = new TrajectoryObservationSettings(); + var settings = new TrajectoryObservationSettings + { + PlanningScope = EmPlanningScope.RollingHorizon, + }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); @@ -1026,6 +1234,18 @@ internal static class TrajectoryObservationChecks } } + private sealed class FailingTrajectoryPlanningService : IEmPlanningService + { + public List Requests { get; } = new List(); + + public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) + { + Requests.Add(request); + return new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null, + "one-shot failure fixture"); + } + } + private static void VerifiesMovementTestKeepsNativePainterLazyAndConditional() { string movementTestPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs index d3189fa..82d900c 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs @@ -1,6 +1,7 @@ using System; using System.IO; using System.Text; +using MultiWheelC.TrajectoryPlanning.EMPlanner; using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; namespace EMPlannerVerificationHost; @@ -13,6 +14,7 @@ internal static class TrajectoryObservationSettingsChecks VerifiesValidatedSnapshotFreezesVisualizationSettings(); RejectsInvalidVisualizationSettings(); VerifiesMovementTestMapsEveryFrozenSetting(); + VerifiesMovementTestFullDirectionScopeMapping(); } private static void VerifiesDefaults() @@ -22,7 +24,9 @@ internal static class TrajectoryObservationSettingsChecks Verification.NearlyEqual(5d, defaults.SolverTimeoutSeconds, "observer solver timeout default"); Verification.Equal(100000, defaults.MaximumOsqpIterations, "observer OSQP default"); Verification.NearlyEqual(2d, defaults.TimeHorizonSeconds, "observer ST horizon default"); - Verification.True(!defaults.EnableWebVisualization, "web defaults off"); + Verification.Equal(EmPlanningScope.FullDirectionSegment, defaults.PlanningScope, + "observer full-direction-segment scope default"); + Verification.True(defaults.EnableWebVisualization, "web defaults on"); Verification.True(defaults.AutoOpenWebVisualization, "web auto-open defaults on"); Verification.True(!defaults.EnableNativePainterVisualization, "Painter defaults off"); Verification.Equal(0, defaults.WebVisualizationPort, "dynamic port default"); @@ -40,6 +44,7 @@ internal static class TrajectoryObservationSettingsChecks { var source = new TrajectoryObservationSettings { + PlanningScope = EmPlanningScope.FullDirectionSegment, EnableWebVisualization = true, AutoOpenWebVisualization = false, WebVisualizationPort = 2048, @@ -54,6 +59,7 @@ internal static class TrajectoryObservationSettingsChecks TrajectoryObservationSettings snapshot = source.CreateValidatedSnapshot(); source.EnableWebVisualization = false; + source.PlanningScope = EmPlanningScope.RollingHorizon; source.AutoOpenWebVisualization = true; source.WebVisualizationPort = 0; source.WebRefreshRateHz = 12d; @@ -64,6 +70,8 @@ internal static class TrajectoryObservationSettingsChecks source.GearSwitchProjectionToleranceMeters = 0.7d; source.GearSwitchStopHoldSeconds = 0.4d; + Verification.Equal(EmPlanningScope.FullDirectionSegment, snapshot.PlanningScope, + "snapshot freezes planning scope"); Verification.True(snapshot.EnableWebVisualization, "snapshot freezes web switch"); Verification.True(!snapshot.AutoOpenWebVisualization, "snapshot freezes web auto-open switch"); Verification.Equal(2048, snapshot.WebVisualizationPort, "snapshot freezes web port"); @@ -84,6 +92,7 @@ internal static class TrajectoryObservationSettingsChecks AssertInvalid(settings => settings.WebVisualizationPort = 1, "low reserved web port"); AssertInvalid(settings => settings.WebVisualizationPort = 1023, "last reserved web port"); AssertInvalid(settings => settings.WebVisualizationPort = 65536, "overflow web port"); + AssertInvalid(settings => settings.PlanningScope = (EmPlanningScope)999, "undefined planning scope"); AssertInvalid(settings => settings.WebRefreshRateHz = 0d, "zero web refresh"); AssertInvalid(settings => settings.VisualizationHistoryCycleLimit = 0, "zero history limit"); AssertInvalid(settings => settings.DirectionConfirmationSpeedMetersPerSecond = 0d, @@ -114,6 +123,21 @@ internal static class TrajectoryObservationSettingsChecks } } + private static void VerifiesMovementTestFullDirectionScopeMapping() + { + string path = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner", + "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs"); + string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(path)); + + Verification.True(source.Contains("public bool UseFullDirectionSegmentPlanning = true;"), + "MovementTest exposes the full-direction-segment switch on by default"); + Verification.True(source.Contains( + "PlanningScope = UseFullDirectionSegmentPlanning ? EmPlanningScope.FullDirectionSegment : EmPlanningScope.RollingHorizon,"), + "MovementTest maps the full-direction-segment switch into settings"); + Verification.True(source.Contains("滚动兼容"), + "MovementTest labels TimeHorizonSeconds as rolling-compatible"); + } + private static void AssertInvalid(Action mutate, string name) { var settings = new TrajectoryObservationSettings();