From 55119de1c73ec737f4c3d467b9abdadb000f2427 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 12:54:53 +0800 Subject: [PATCH] feat: select safe EM trajectory handoffs --- .../EmPlanningCoordinator.cs | 15 ++ .../TrajectoryHandoffSelector.cs | 171 ++++++++++++++++++ .../TrajectoryExecution/TrajectorySampler.cs | 85 +++++++++ .../CoordinatorChecks.cs | 143 ++++++++++++++- 4 files changed, 409 insertions(+), 5 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs index f479bab..6a0fcfa 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs @@ -1,6 +1,7 @@ using System; using System.Threading; using System.Threading.Tasks; +using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -12,6 +13,7 @@ public sealed class EmPlanningCoordinator { private readonly IEmPlanningService planningService; private readonly IEmPlanningCycleSink sink; + private readonly TrajectoryHandoffSelector handoffSelector = new TrajectoryHandoffSelector(); private readonly object publicationGate = new object(); private long latestCycleVersion; private PlanningCycleIdentity latestIdentity; @@ -66,6 +68,19 @@ public sealed class EmPlanningCoordinator return Task.Run(() => CompleteCycle(version, input, cycleCancellation)); } + /// Builds a handoff decision from the coordinator's immutable published trajectory snapshot. + public TrajectoryHandoffSelection SelectHandoff(PlanningCycleInput input, TravelDirection expectedDirection) + { + if (input == null) + throw new ArgumentNullException(nameof(input)); + + EmTrajectory trajectory; + lock (publicationGate) + trajectory = publishedTrajectory; + return handoffSelector.Select(trajectory, input.Request.VehicleState, input.Identity.SegmentIndex, expectedDirection, + input.Now, input.Request.Configuration); + } + private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input, CancellationTokenSource cycleCancellation) { diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs new file mode 100644 index 0000000..3704347 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs @@ -0,0 +1,171 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum TrajectoryHandoffSource +{ + MeasuredState, + PreviousTrajectory, +} + +public enum TrajectoryHandoffRejectionReason +{ + None, + MissingTrajectory, + InvalidConfiguration, + TrajectoryNotYetEffective, + TrajectoryTooOld, + SegmentMismatch, + DirectionMismatch, + TrackingErrorExceeded, + GearBoundary, + TerminalBoundary, + HandoffBeyondTrajectory, + SampleUnavailable, +} + +/// Immutable handoff decision for the next one-shot planning request. +public sealed class TrajectoryHandoffSelection +{ + internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState, + EmTrajectory previousTrajectory, EmTrajectoryPoint sampledPoint, TrajectoryHandoffRejectionReason rejectionReason) + { + Source = source; + StartState = startState ?? throw new ArgumentNullException(nameof(startState)); + PreviousTrajectory = previousTrajectory; + SampledPoint = sampledPoint; + RejectionReason = rejectionReason; + } + + public TrajectoryHandoffSource Source { get; } + + public VehicleMotionState StartState { get; } + + public EmTrajectory PreviousTrajectory { get; } + + public EmTrajectoryPoint SampledPoint { get; } + + public TrajectoryHandoffRejectionReason RejectionReason { get; } +} + +/// Selects a future same-segment seed or preserves the caller's measured state. +public sealed class TrajectoryHandoffSelector +{ + private const double TimeEpsilonSeconds = 1e-9d; + private readonly TrajectorySampler sampler; + + public TrajectoryHandoffSelector() + : this(new TrajectorySampler()) + { + } + + internal TrajectoryHandoffSelector(TrajectorySampler sampler) + { + this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler)); + } + + public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState, + int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now, + EmPlannerConfiguration configuration) + { + if (measuredState == null) + throw new ArgumentNullException(nameof(measuredState)); + if (expectedSegmentIndex < 0 || !Enum.IsDefined(typeof(TravelDirection), expectedDirection)) + throw new ArgumentOutOfRangeException(nameof(expectedSegmentIndex)); + if (previousTrajectory == null) + return Measured(measuredState, TrajectoryHandoffRejectionReason.MissingTrajectory); + if (!TryReadConfiguration(configuration, out double maximumAgeSeconds, out double lookaheadSeconds, + out double spatialToleranceMeters, out double kinematicTolerance)) + { + return Measured(measuredState, TrajectoryHandoffRejectionReason.InvalidConfiguration); + } + if (previousTrajectory.Metadata.SegmentIndex != expectedSegmentIndex) + return Measured(measuredState, TrajectoryHandoffRejectionReason.SegmentMismatch); + if (previousTrajectory.Metadata.Direction != expectedDirection) + return Measured(measuredState, TrajectoryHandoffRejectionReason.DirectionMismatch); + + double trajectoryAgeSeconds = (now - previousTrajectory.Metadata.EffectiveAtUtc).TotalSeconds; + if (trajectoryAgeSeconds < -TimeEpsilonSeconds) + return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryNotYetEffective); + if (trajectoryAgeSeconds > maximumAgeSeconds + TimeEpsilonSeconds) + return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryTooOld); + if (!sampler.TrySample(previousTrajectory, trajectoryAgeSeconds, out EmTrajectoryPoint currentPoint)) + return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable); + if (!Tracks(currentPoint, measuredState, spatialToleranceMeters, kinematicTolerance)) + return Measured(measuredState, TrajectoryHandoffRejectionReason.TrackingErrorExceeded); + + double handoffTime = trajectoryAgeSeconds + lookaheadSeconds; + double terminalTime = previousTrajectory.Points[previousTrajectory.Points.Count - 1].TimeFromStart; + if (handoffTime > terminalTime + TimeEpsilonSeconds) + return Measured(measuredState, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory); + TrajectoryHandoffRejectionReason boundaryReason = BoundaryInInterval(previousTrajectory, trajectoryAgeSeconds, + handoffTime); + if (boundaryReason != TrajectoryHandoffRejectionReason.None) + return Measured(measuredState, boundaryReason); + if (!sampler.TrySample(previousTrajectory, handoffTime, out EmTrajectoryPoint handoffPoint)) + return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable); + + var startState = new VehicleMotionState(new Pose2D(handoffPoint.X, handoffPoint.Y, handoffPoint.Yaw), + handoffPoint.SignedLongitudinalVelocity, handoffPoint.LongitudinalAcceleration, + previousTrajectory.Metadata.EffectiveAtUtc.AddSeconds(handoffTime), measuredState.SequenceId); + return new TrajectoryHandoffSelection(TrajectoryHandoffSource.PreviousTrajectory, startState, + previousTrajectory, handoffPoint, TrajectoryHandoffRejectionReason.None); + } + + private static TrajectoryHandoffSelection Measured(VehicleMotionState measuredState, + TrajectoryHandoffRejectionReason rejectionReason) + { + return new TrajectoryHandoffSelection(TrajectoryHandoffSource.MeasuredState, measuredState, null, null, + rejectionReason); + } + + private static bool TryReadConfiguration(EmPlannerConfiguration configuration, out double maximumAgeSeconds, + out double lookaheadSeconds, out double spatialToleranceMeters, out double kinematicTolerance) + { + maximumAgeSeconds = configuration?.Scheduling?.MaximumVehicleStateAgeSeconds ?? double.NaN; + lookaheadSeconds = configuration?.Scheduling?.HandoffLookaheadSeconds ?? double.NaN; + spatialToleranceMeters = configuration?.Validation?.SpatialToleranceMeters ?? double.NaN; + kinematicTolerance = configuration?.Validation?.KinematicTolerance ?? double.NaN; + return IsNonNegativeFinite(maximumAgeSeconds) && IsNonNegativeFinite(lookaheadSeconds) && + IsNonNegativeFinite(spatialToleranceMeters) && IsNonNegativeFinite(kinematicTolerance); + } + + private static bool Tracks(EmTrajectoryPoint trajectoryPoint, VehicleMotionState measuredState, + double spatialToleranceMeters, double kinematicTolerance) + { + double dx = trajectoryPoint.X - measuredState.Pose.X; + double dy = trajectoryPoint.Y - measuredState.Pose.Y; + if (Math.Sqrt(dx * dx + dy * dy) > spatialToleranceMeters) + return false; + double yawError = Math.Atan2(Math.Sin(trajectoryPoint.Yaw - measuredState.Pose.Heading), + Math.Cos(trajectoryPoint.Yaw - measuredState.Pose.Heading)); + return Math.Abs(yawError) <= kinematicTolerance && + Math.Abs(trajectoryPoint.SignedLongitudinalVelocity - + measuredState.SignedLongitudinalSpeedMetersPerSecond) <= kinematicTolerance; + } + + private static TrajectoryHandoffRejectionReason BoundaryInInterval(EmTrajectory trajectory, double startTime, + double endTime) + { + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (point.TimeFromStart <= startTime + TimeEpsilonSeconds || point.TimeFromStart > endTime + TimeEpsilonSeconds) + continue; + if (point.BoundaryType == EmBoundaryType.GearSwitchApproach || + point.BoundaryType == EmBoundaryType.GearSwitchDeparture) + { + return TrajectoryHandoffRejectionReason.GearBoundary; + } + if (point.BoundaryType == EmBoundaryType.RollingSafetyStop || point.BoundaryType == EmBoundaryType.Goal) + return TrajectoryHandoffRejectionReason.TerminalBoundary; + } + return TrajectoryHandoffRejectionReason.None; + } + + private static bool IsNonNegativeFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs new file mode 100644 index 0000000..25552bd --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs @@ -0,0 +1,85 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Interpolates an immutable trajectory only inside one homogeneous trajectory interval. +public sealed class TrajectorySampler +{ + private const double TimeEpsilonSeconds = 1e-9d; + + public bool TrySample(EmTrajectory trajectory, double timeFromStart, out EmTrajectoryPoint sampledPoint) + { + sampledPoint = null; + if (trajectory == null || double.IsNaN(timeFromStart) || double.IsInfinity(timeFromStart) || + trajectory.Points.Count == 0) + { + return false; + } + + EmTrajectoryPoint first = trajectory.Points[0]; + EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1]; + if (timeFromStart < first.TimeFromStart - TimeEpsilonSeconds || + timeFromStart > last.TimeFromStart + TimeEpsilonSeconds) + { + return false; + } + + int lower = 0; + int upper = trajectory.Points.Count - 1; + while (upper - lower > 1) + { + int middle = lower + (upper - lower) / 2; + EmTrajectoryPoint point = trajectory.Points[middle]; + if (Math.Abs(point.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds) + { + sampledPoint = point; + return true; + } + + if (point.TimeFromStart < timeFromStart) + lower = middle; + else + upper = middle; + } + + EmTrajectoryPoint left = trajectory.Points[lower]; + EmTrajectoryPoint right = trajectory.Points[upper]; + if (Math.Abs(left.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds) + { + sampledPoint = left; + return true; + } + if (Math.Abs(right.TimeFromStart - timeFromStart) <= TimeEpsilonSeconds) + { + sampledPoint = right; + return true; + } + if (left.SegmentIndex != right.SegmentIndex || left.Direction != right.Direction || + left.BoundaryType != right.BoundaryType) + { + return false; + } + + double fraction = (timeFromStart - left.TimeFromStart) / (right.TimeFromStart - left.TimeFromStart); + sampledPoint = new EmTrajectoryPoint( + Interpolate(left.X, right.X, fraction), + Interpolate(left.Y, right.Y, fraction), + Interpolate(left.Yaw, right.Yaw, fraction), + Interpolate(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, fraction), + timeFromStart, + Interpolate(left.VehicleCurvature, right.VehicleCurvature, fraction), + left.SegmentIndex, + Interpolate(left.SegmentLocalS, right.SegmentLocalS, fraction), + Interpolate(left.PathS, right.PathS, fraction), + left.Direction, + left.BoundaryType, + Interpolate(left.LongitudinalAcceleration, right.LongitudinalAcceleration, fraction), + Interpolate(left.LongitudinalJerk, right.LongitudinalJerk, fraction)); + return true; + } + + private static double Interpolate(double left, double right, double fraction) + { + return left + (right - left) * fraction; + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs index 807f97a..5d5ca15 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs @@ -16,6 +16,7 @@ internal static class CoordinatorChecks VerifiesCompletedCycleDoesNotPoisonNextCancellationSource(); VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults(); VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics(); + VerifiesSafePreviousTrajectoryHandoffs(); } private static void VerifiesCallerSuppliedSchedulingDecision() @@ -96,6 +97,134 @@ internal static class CoordinatorChecks "sink failure is reported in diagnostic"); } + private static void VerifiesSafePreviousTrajectoryHandoffs() + { + DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(300d); + EmPlannerConfiguration configuration = CreateHandoffConfiguration(); + var selector = new TrajectoryHandoffSelector(); + EmTrajectory forward = CreateHandoffTrajectory("handoff-forward", TravelDirection.Forward, 2, effectiveAt, + EmBoundaryType.RollingSafetyStop, false); + VehicleMotionState forwardMeasured = CreateMeasuredState(0.01d, 0d, 3.1666666666666665d, 0.10d, + effectiveAt.AddSeconds(0.10d), 40L); + + TrajectoryHandoffSelection accepted = selector.Select(forward, forwardMeasured, 2, TravelDirection.Forward, + effectiveAt.AddSeconds(0.10d), configuration); + Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, accepted.Source, "forward handoff source"); + Verification.Equal(TrajectoryHandoffRejectionReason.None, accepted.RejectionReason, "forward handoff reason"); + Verification.True(object.ReferenceEquals(forward, accepted.PreviousTrajectory), "forward handoff seed"); + Verification.NearlyEqual(0.04d, accepted.StartState.Pose.X, "forward handoff position interpolation"); + Verification.NearlyEqual(3.3666666666666667d, accepted.StartState.Pose.Heading, + "forward handoff yaw remains unwrapped"); + Verification.NearlyEqual(0.10d, accepted.StartState.SignedLongitudinalSpeedMetersPerSecond, + "forward handoff signed speed interpolation"); + Verification.Equal(40L, accepted.StartState.SequenceId, "handoff retains measured-state identity"); + + EmTrajectory reverse = CreateHandoffTrajectory("handoff-reverse", TravelDirection.Reverse, 2, effectiveAt, + EmBoundaryType.RollingSafetyStop, false); + VehicleMotionState reverseMeasured = CreateMeasuredState(-0.01d, 0d, 3.1666666666666665d, -0.10d, + effectiveAt.AddSeconds(0.10d), 41L); + TrajectoryHandoffSelection reverseAccepted = selector.Select(reverse, reverseMeasured, 2, TravelDirection.Reverse, + effectiveAt.AddSeconds(0.10d), configuration); + Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, reverseAccepted.Source, + "reverse same-segment handoff source"); + Verification.NearlyEqual(-0.04d, reverseAccepted.StartState.Pose.X, "reverse handoff position interpolation"); + Verification.NearlyEqual(-0.10d, reverseAccepted.StartState.SignedLongitudinalSpeedMetersPerSecond, + "reverse handoff signed speed interpolation"); + + AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Forward, + effectiveAt.AddSeconds(0.21d), configuration, TrajectoryHandoffRejectionReason.TrajectoryTooOld, + "stale trajectory handoff"); + configuration.Scheduling.MaximumVehicleStateAgeSeconds = 0.60d; + AssertHandoffRejected(selector, forward, + CreateMeasuredState(1d, 0d, 3.1666666666666665d, 0.10d, effectiveAt.AddSeconds(0.10d), 42L), 2, + TravelDirection.Forward, effectiveAt.AddSeconds(0.10d), configuration, + TrajectoryHandoffRejectionReason.TrackingErrorExceeded, "large tracking-error handoff"); + VehicleMotionState terminalMeasured = CreateMeasuredState(0.03d, 0d, 3.30d, 0.10d, + effectiveAt.AddSeconds(0.30d), 43L); + AssertHandoffRejected(selector, forward, terminalMeasured, 2, TravelDirection.Forward, + effectiveAt.AddSeconds(0.30d), configuration, TrajectoryHandoffRejectionReason.TerminalBoundary, + "terminal-proximity handoff"); + AssertHandoffRejected(selector, forward, forwardMeasured, 3, TravelDirection.Forward, + effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.SegmentMismatch, + "segment-mismatch handoff"); + AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Reverse, + effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.DirectionMismatch, + "direction-mismatch handoff"); + VehicleMotionState beyondMeasured = CreateMeasuredState(0.031d, 0d, 3.3066666666666666d, 0.10d, + effectiveAt.AddSeconds(0.31d), 44L); + AssertHandoffRejected(selector, forward, beyondMeasured, 2, TravelDirection.Forward, + effectiveAt.AddSeconds(0.31d), configuration, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory, + "beyond-trajectory handoff"); + + EmTrajectory gearBoundary = CreateHandoffTrajectory("handoff-gear", TravelDirection.Forward, 2, effectiveAt, + EmBoundaryType.RollingSafetyStop, true); + AssertHandoffRejected(selector, gearBoundary, forwardMeasured, 2, TravelDirection.Forward, + effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.GearBoundary, + "gear-boundary handoff"); + Verification.True(!new TrajectorySampler().TrySample(gearBoundary, 0.15d, out _), + "sampler never interpolates across different boundary types"); + + var service = new ControlledPlanningService(); + var coordinator = new EmPlanningCoordinator(service); + PlanningCycleInput coordinatorInput = CreateInput(CreateMap(22), "handoff-reference", 40L, "handoff-forward", + 2, "handoff-publish", effectiveAt.AddSeconds(0.10d), configuration, forwardMeasured); + Task publication = coordinator.PlanLatestAsync(coordinatorInput, CancellationToken.None); + service.WaitUntilStarted(coordinatorInput.Request.OutputTrajectoryId); + service.Complete(coordinatorInput.Request.OutputTrajectoryId, forward); + publication.GetAwaiter().GetResult(); + TrajectoryHandoffSelection coordinatorSelection = coordinator.SelectHandoff(coordinatorInput, TravelDirection.Forward); + Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, coordinatorSelection.Source, + "coordinator consumes only its published immutable trajectory"); + } + + private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory, + VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now, + EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name) + { + TrajectoryHandoffSelection selection = selector.Select(trajectory, measuredState, segmentIndex, direction, now, + configuration); + Verification.Equal(TrajectoryHandoffSource.MeasuredState, selection.Source, name + " source"); + Verification.Equal(reason, selection.RejectionReason, name + " reason"); + Verification.True(selection.PreviousTrajectory == null, name + " has no seed"); + Verification.True(object.ReferenceEquals(measuredState, selection.StartState), name + " returns measured state"); + } + + private static EmPlannerConfiguration CreateHandoffConfiguration() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Validation.SpatialToleranceMeters = 0.05d; + configuration.Validation.KinematicTolerance = 0.05d; + return configuration; + } + + private static VehicleMotionState CreateMeasuredState(double x, double y, double yaw, double signedSpeed, + DateTimeOffset capturedAt, long sequenceId) + { + return new VehicleMotionState(new Pose2D(x, y, yaw), signedSpeed, 0d, capturedAt, sequenceId); + } + + private static EmTrajectory CreateHandoffTrajectory(string trajectoryId, TravelDirection direction, int segmentIndex, + DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, bool includeGearBoundary) + { + double sign = direction == TravelDirection.Forward ? 1d : -1d; + EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None; + var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L, + string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop); + return new EmTrajectory(metadata, new[] + { + new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.01d, 0d, 3.1666666666666665d, sign * 0.10d, 0.10d, 0.25d, + segmentIndex, 0.01d, 0.01d, direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.03d, 0d, 3.30d, sign * 0.10d, 0.30d, 0.25d, segmentIndex, 0.03d, + 0.03d, direction, middleBoundary, 0d, 0d), + new EmTrajectoryPoint(sign * 0.055d, 0d, 3.4666666666666668d, sign * 0.10d, 0.55d, 0.25d, + segmentIndex, 0.055d, 0.055d, direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.06d, 0d, 3.50d, 0d, 0.60d, 0.25d, segmentIndex, 0.06d, 0.06d, + direction, terminalBoundary, 0d, 0d), + }); + } + private static void VerifySuperseded(string name, PlanningCycleInput older, PlanningCycleInput newer) { var service = new ControlledPlanningService(); @@ -121,10 +250,11 @@ internal static class CoordinatorChecks } private static PlanningCycleInput CreateInput(PlanningGridMap map, string referencePathId, long stateSequenceId, - string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now) + string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now, + EmPlannerConfiguration? configuration = null, VehicleMotionState? state = null) { - EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); - var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId); + configuration ??= EmPlannerConfiguration.CreateDefault(); + state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId); var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now, outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse); return new PlanningCycleInput(request, now); @@ -179,12 +309,15 @@ internal static class CoordinatorChecks return pending[outputTrajectoryId].CancellationToken.IsCancellationRequested; } - public void Complete(string outputTrajectoryId) + public void Complete(string outputTrajectoryId, EmTrajectory? trajectory = null) { PendingCycle cycle; lock (gate) cycle = pending[outputTrajectoryId]; - cycle.Completion.TrySetResult(CreateSuccess(cycle.Request)); + EmPlanningResult result = trajectory == null + ? CreateSuccess(cycle.Request) + : new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty); + cycle.Completion.TrySetResult(result); } private static EmPlanningResult CreateSuccess(EmPlanningRequest request)