diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs new file mode 100644 index 0000000..df63806 --- /dev/null +++ b/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 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 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(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason); + } + + public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job, + CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, + IReadOnlyList 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 CopySegments(IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) + copy.Add(source[index]); + } + return new ReadOnlyCollection(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 CopyFiniteClearance(IReadOnlyList 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(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(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 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 lsSamples, + IReadOnlyList stSamples, + IReadOnlyList speedSamples, int failedProjectionCount) + { + LsSamples = new ReadOnlyCollection( + new List(lsSamples)); + StSamples = new ReadOnlyCollection( + new List(stSamples)); + SpeedSamples = new ReadOnlyCollection( + new List(speedSamples)); + FailedProjectionCount = failedProjectionCount; + } + + public IReadOnlyList LsSamples { get; } + + public IReadOnlyList StSamples { get; } + + public IReadOnlyList 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(trajectory.Points.Count); + var st = new List(trajectory.Points.Count); + var speed = new List(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); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 288f28d..060076c 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/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(), 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 + { + 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 Requests { get; } = new List(); + + 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; }