feat: observe one full EM direction segment

2026-08-07 08:56:14 +08:00
parent 57d8abb3e1
commit ab3204bfde
6 changed files with 289 additions and 20 deletions
@@ -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<object>())!;
+        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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 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<TrajectoryObservationObstacle>(), 0L);
@@ -1026,6 +1234,18 @@ internal static class TrajectoryObservationChecks
        }
    }

+    private sealed class FailingTrajectoryPlanningService : IEmPlanningService
+    {
+        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();
+
+        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",
@@ -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<TrajectoryObservationSettings> mutate, string name)
    {
        var settings = new TrajectoryObservationSettings();