feat: select safe EM trajectory handoffs

2026-08-04 12:54:53 +08:00
parent d75380cf8f
commit 55119de1c7
4 changed files with 409 additions and 5 deletions
@@ -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));
    }

+    /// <summary>Builds a handoff decision from the coordinator's immutable published trajectory snapshot.</summary>
+    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)
    {
@@ -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,
+}
+
+/// <summary>Immutable handoff decision for the next one-shot planning request.</summary>
+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; }
+}
+
+/// <summary>Selects a future same-segment seed or preserves the caller's measured state.</summary>
+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;
+    }
+}
@@ -0,0 +1,85 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Interpolates an immutable trajectory only inside one homogeneous trajectory interval.</summary>
+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;
+    }
+}