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