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