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)