feat: add EM observation planning pipeline

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs

@@ -0,0 +1,335 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using System.Threading.Tasks;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+public sealed class TrajectoryObservationBootstrapResult
+{
+    private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
+        PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
+    {
+        Job = job ?? throw new ArgumentNullException(nameof(job));
+        CoarseResult = coarse;
+        SmoothedPath = smoothedPath;
+        Segments = CopySegments(segments);
+        FailureReason = failureReason ?? string.Empty;
+        Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
+    }
+
+    public bool Succeeded { get; }
+
+    public CoarsePathPlanningJob Job { get; }
+
+    public CoarsePathPlanningJobResult CoarseResult { get; }
+
+    public PathSmoothingResult SmoothedPath { get; }
+
+    public IReadOnlyList<DirectionSegmentView> Segments { get; }
+
+    public string FailureReason { get; }
+
+    public PlanningGridMap Map => CoarseResult?.MapResult?.Map;
+
+    public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
+    {
+        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
+            Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
+    }
+
+    public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
+        IReadOnlyList<DirectionSegmentView> segments)
+    {
+        if (coarse == null) throw new ArgumentNullException(nameof(coarse));
+        if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
+        return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
+    }
+
+    private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
+    {
+        var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++)
+                copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<DirectionSegmentView>(copy);
+    }
+}
+
+public sealed class TrajectoryObservationBootstrapper
+{
+    private readonly CoarsePathPlanningService coarseService;
+    private readonly PathSmoothingService smoothingService;
+
+    public TrajectoryObservationBootstrapper()
+        : this(new CoarsePathPlanningService(), new PathSmoothingService())
+    {
+    }
+
+    public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
+        PathSmoothingService smoothingService)
+    {
+        this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
+        this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
+    }
+
+    public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
+        CancellationToken cancellationToken = default)
+    {
+        if (job == null) throw new ArgumentNullException(nameof(job));
+
+        CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
+        if (coarse.PlanningResult.Status != PlanningStatus.Success)
+            return TrajectoryObservationBootstrapResult.FromFailure(
+                job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
+
+        var smoothingRequest = new PathSmoothingRequest(
+            CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
+            coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
+            new PathSmoothingConfiguration());
+        PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
+        if (!IsPublishedSmoothingStatus(smooth.Status))
+            return TrajectoryObservationBootstrapResult.FromFailure(
+                job, coarse, smooth, smooth.Diagnostics.TerminationReason);
+        return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
+    }
+
+    private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
+        PlanningGridMap map)
+    {
+        if (path == null) throw new ArgumentNullException(nameof(path));
+        if (map == null) throw new ArgumentNullException(nameof(map));
+
+        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
+        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
+        double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
+        var copy = new List<CoarsePathPoint>(path.Count);
+        for (int index = 0; index < path.Count; index++)
+        {
+            CoarsePathPoint point = path[index] ?? throw new ArgumentException(
+                "The coarse path cannot contain null points.", nameof(path));
+            double clearance = double.IsPositiveInfinity(point.BodyClearance)
+                ? mapDiagonalMeters
+                : point.BodyClearance;
+            copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
+                point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
+                point.IsGearSwitchPoint, point.Source));
+        }
+        return new ReadOnlyCollection<CoarsePathPoint>(copy);
+    }
+
+    private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
+    {
+        return status == PathSmoothingStatus.Complete ||
+            status == PathSmoothingStatus.PartialImprovement ||
+            status == PathSmoothingStatus.NotNeeded ||
+            status == PathSmoothingStatus.Unchanged;
+    }
+}
+
+public sealed class TrajectoryObservationObservation
+{
+    internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
+        EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
+        TrajectoryExecutionState executorState)
+    {
+        ObservedAtUtc = observedAtUtc;
+        VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
+        PublishedTrajectory = publishedTrajectory;
+        SelectedPoint = selectedPoint;
+        Command = command;
+        ExecutorState = executorState;
+    }
+
+    public DateTimeOffset ObservedAtUtc { get; }
+
+    public VehicleMotionState VehicleState { get; }
+
+    public EmTrajectory PublishedTrajectory { get; }
+
+    public EmTrajectoryPoint SelectedPoint { get; }
+
+    public TrajectoryControlCommand Command { get; }
+
+    public TrajectoryExecutionState ExecutorState { get; }
+}
+
+public sealed class TrajectoryObservationController
+{
+    private readonly TrajectoryObservationBootstrapResult bootstrap;
+    private readonly EmPlannerConfiguration configuration;
+    private readonly EmPlanningCoordinator coordinator;
+    private readonly TrajectoryExecutor executor;
+    private readonly string sessionId;
+    private long cycleId;
+
+    public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
+        TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
+    {
+        this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
+        if (!bootstrap.Succeeded)
+            throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
+        if (settings == null) throw new ArgumentNullException(nameof(settings));
+        if (planningService == null) throw new ArgumentNullException(nameof(planningService));
+        if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));
+
+        settings.Validate();
+        configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds;
+        coordinator = new EmPlanningCoordinator(planningService);
+        executor = new TrajectoryExecutor(configuration);
+        this.sessionId = sessionId;
+    }
+
+    public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
+
+    public bool ShouldStartCycle(DateTimeOffset now)
+    {
+        return coordinator.ShouldStartCycle(now);
+    }
+
+    public Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
+        CancellationToken cancellationToken)
+    {
+        if (state == null) throw new ArgumentNullException(nameof(state));
+
+        const int segmentIndex = 0;
+        long currentCycleId = Interlocked.Increment(ref cycleId);
+        var request = new EmPlanningRequest(
+            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Job.Vehicle, state, configuration,
+            segmentIndex, coordinator.PublishedTrajectory, now, now,
+            sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
+            coordinator.PublishedTrajectory?.Metadata.TrajectoryId ?? string.Empty,
+            EmMotionModel.NonholonomicForwardReverse);
+        return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
+    }
+
+    public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
+    {
+        if (state == null) throw new ArgumentNullException(nameof(state));
+
+        EmTrajectory trajectory = coordinator.PublishedTrajectory;
+        if (trajectory == null)
+            return new TrajectoryObservationObservation(now, state, null, null, null, null);
+
+        TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
+            trajectory.Metadata.Direction, trajectory.Metadata.Direction, true);
+        TrajectoryExecutionState executorState = executor.State;
+        return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
+            command, executorState);
+    }
+}
+
+public sealed class TrajectoryObservationLsSample
+{
+    public TrajectoryObservationLsSample(double pathS, double lateralOffset)
+    {
+        PathS = pathS;
+        LateralOffset = lateralOffset;
+    }
+
+    public double PathS { get; }
+
+    public double LateralOffset { get; }
+}
+
+public sealed class TrajectoryObservationStSample
+{
+    public TrajectoryObservationStSample(double timeFromStart, double pathS)
+    {
+        TimeFromStart = timeFromStart;
+        PathS = pathS;
+    }
+
+    public double TimeFromStart { get; }
+
+    public double PathS { get; }
+}
+
+public sealed class TrajectoryObservationSpeedSample
+{
+    public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
+    {
+        TimeFromStart = timeFromStart;
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+    }
+
+    public double TimeFromStart { get; }
+
+    public double SignedLongitudinalVelocity { get; }
+}
+
+public sealed class TrajectoryObservationCharts
+{
+    private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
+        IReadOnlyList<TrajectoryObservationStSample> stSamples,
+        IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
+    {
+        LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
+            new List<TrajectoryObservationLsSample>(lsSamples));
+        StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
+            new List<TrajectoryObservationStSample>(stSamples));
+        SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
+            new List<TrajectoryObservationSpeedSample>(speedSamples));
+        FailedProjectionCount = failedProjectionCount;
+    }
+
+    public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }
+
+    public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }
+
+    public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }
+
+    public int FailedProjectionCount { get; }
+
+    public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
+        double maximumProjectionDistanceMeters)
+    {
+        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
+        if (segment == null) throw new ArgumentNullException(nameof(segment));
+        if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
+            maximumProjectionDistanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));
+
+        var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
+        var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
+        var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
+        var projector = new FrenetProjector();
+        double seedReferenceS = 0d;
+        int failedProjectionCount = 0;
+        for (int index = 0; index < trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = trajectory.Points[index];
+            var pose = new Pose2D(point.X, point.Y, point.Yaw);
+            if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
+                maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
+            {
+                ls.Add(new TrajectoryObservationLsSample(
+                    segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
+                seedReferenceS = projection.ReferenceS;
+            }
+            else
+            {
+                failedProjectionCount++;
+            }
+
+            st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
+            speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
+        }
+
+        return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -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; }