feat: add EM observation planning pipeline

2026-08-04 15:57:59 +08:00
parent 5df198bf69
commit 0f5255c598
2 changed files with 442 additions and 0 deletions
@@ -1,6 +1,11 @@
 using System;
+using System.Collections.Generic;
+using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
 using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

 namespace EMPlannerVerificationHost;
@@ -11,6 +16,7 @@ internal static class TrajectoryObservationChecks
    {
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
        RejectsObstacleOutsideConfiguredBounds();
+        VerifiesLsAndStUsePublishedTrajectoryData();
    }

    private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
@@ -39,6 +45,107 @@ internal static class TrajectoryObservationChecks
            "observer obstacle outside configured bounds");
    }

+    private static void VerifiesLsAndStUsePublishedTrajectoryData()
+    {
+        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
+        DirectionSegmentView segment = CreateStraightSegment();
+        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
+
+        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
+        Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
+        Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
+        Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
+        Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
+        Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
+        Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
+        Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
+            "observer first signed speed");
+        Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
+            "observer second signed speed");
+        Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
+        Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
+        Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
+        Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");
+
+        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(
+            new CoarsePathPlanningService(), new PathSmoothingService())
+            .Bootstrap(job, CancellationToken.None);
+        Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");
+
+        var planningService = new FixedTrajectoryPlanningService(trajectory);
+        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
+        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
+        PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
+            .GetAwaiter().GetResult();
+        Verification.True(firstCycle.Published, "observer first cycle publishes");
+        Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
+        Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
+            "observer first trajectory identity");
+        Verification.NearlyEqual(settings.ReplanPeriodSeconds,
+            planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
+            "observer configured replan period");
+
+        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
+        Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
+            "observer executor interpolates from published effective time");
+
+        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");
+    }
+
+    private static DirectionSegmentView CreateStraightSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
+                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
+                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+        };
+        return new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
+            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
+    }
+
+    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt)
+    {
+        var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
+            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
+                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
+                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
+        });
+    }
+
+    private sealed class FixedTrajectoryPlanningService : IEmPlanningService
+    {
+        private readonly EmTrajectory trajectory;
+
+        public FixedTrajectoryPlanningService(EmTrajectory trajectory)
+        {
+            this.trajectory = trajectory;
+        }
+
+        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();
+
+        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+        {
+            Requests.Add(request);
+            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
+        }
+    }
+
    private static bool Throws(Action action)
    {
        try { action(); return false; }