feat: assemble complete EM trajectories

2026-08-04 11:35:50 +08:00
parent 591143f181
commit 1d58864908
5 changed files with 391 additions and 1 deletions
@@ -0,0 +1,84 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds immutable world-space trajectory points from validated LS/ST results.</summary>
+public sealed class EmTrajectoryAssembler
+{
+    private readonly double outputTimeStepSeconds;
+    private readonly double zeroSpeedHoldSeconds;
+
+    public EmTrajectoryAssembler()
+        : this(EmPlannerConfiguration.CreateDefault())
+    {
+    }
+
+    public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
+    {
+        if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
+            throw new ArgumentNullException(nameof(configuration));
+        outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
+        zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
+        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
+            zeroSpeedHoldSeconds < 0d)
+        {
+            throw new ArgumentOutOfRangeException(nameof(configuration));
+        }
+    }
+
+    public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
+    {
+        if (longitudinal == null || longitudinal.Candidate == null ||
+            (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
+        {
+            throw new ArgumentException("Trajectory assembly requires a successful longitudinal result.", nameof(longitudinal));
+        }
+        if (metadata == null)
+            throw new ArgumentNullException(nameof(metadata));
+
+        var interpolator = new LateralPathInterpolator(path);
+        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds);
+        double terminalPathS = path.Points[path.Points.Count - 1].PathS;
+        var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
+        double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
+
+        for (int index = 0; index < schedule.Samples.Count; index++)
+        {
+            TrajectorySample sample = schedule.Samples[index];
+            if (sample.PathS > terminalPathS + 1e-10d)
+                throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal));
+
+            InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS);
+            bool isTerminalAnchor = index == longitudinal.Candidate.KnotTimes.Count - 1;
+            EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
+            double signedSpeed = directionSign * sample.ProgressSpeed;
+            points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
+                geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
+                boundaryType, sample.Acceleration, sample.Jerk));
+        }
+
+        return new EmTrajectory(metadata, points);
+    }
+
+    private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)
+    {
+        switch (terminalType)
+        {
+            case EmTerminalType.RollingSafetyStop:
+                return EmBoundaryType.RollingSafetyStop;
+            case EmTerminalType.GearSwitch:
+                return EmBoundaryType.GearSwitchApproach;
+            case EmTerminalType.Goal:
+                return EmBoundaryType.Goal;
+            default:
+                throw new ArgumentOutOfRangeException(nameof(terminalType));
+        }
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -0,0 +1,99 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LateralPathInterpolator
+{
+    private const double BoundaryTolerance = 1e-10d;
+    private readonly LateralPath path;
+
+    public LateralPathInterpolator(LateralPath path)
+    {
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
+            throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path));
+
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < path.Points.Count; index++)
+        {
+            LateralPathPoint point = path.Points[index];
+            if (point == null || point.PathS <= previousPathS)
+                throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path));
+            previousPathS = point.PathS;
+        }
+
+        this.path = path;
+    }
+
+    public InterpolatedLateralPathPoint Interpolate(double pathS)
+    {
+        if (!IsFinite(pathS))
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        LateralPathPoint first = path.Points[0];
+        LateralPathPoint last = path.Points[path.Points.Count - 1];
+        if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (pathS <= first.PathS + BoundaryTolerance)
+            return From(first);
+        if (pathS >= last.PathS - BoundaryTolerance)
+            return From(last);
+
+        for (int index = 1; index < path.Points.Count; index++)
+        {
+            LateralPathPoint right = path.Points[index];
+            if (pathS <= right.PathS)
+            {
+                LateralPathPoint left = path.Points[index - 1];
+                double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
+                return new InterpolatedLateralPathPoint(
+                    Linear(left.X, right.X, ratio),
+                    Linear(left.Y, right.Y, ratio),
+                    NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
+                    Linear(left.VehicleCurvature, right.VehicleCurvature, ratio));
+            }
+        }
+
+        throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed.");
+    }
+
+    private static InterpolatedLateralPathPoint From(LateralPathPoint point)
+    {
+        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
+    }
+
+    private static double Linear(double left, double right, double ratio)
+    {
+        return left + ratio * (right - left);
+    }
+
+    internal static double NormalizeYaw(double yaw)
+    {
+        double normalized = yaw % (2d * Math.PI);
+        if (normalized >= Math.PI)
+            normalized -= 2d * Math.PI;
+        if (normalized < -Math.PI)
+            normalized += 2d * Math.PI;
+        return normalized;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class InterpolatedLateralPathPoint
+{
+    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
+    {
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        VehicleCurvature = vehicleCurvature;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+    public double Yaw { get; }
+    public double VehicleCurvature { get; }
+}
@@ -0,0 +1,74 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class TrajectorySampleSchedule
+{
+    private const double ZeroTolerance = 1e-12d;
+
+    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
+    {
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
+        if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
+            throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
+        if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
+            throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
+
+        var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < candidate.KnotTimes.Count; index++)
+        {
+            if (candidate.S[index] < previousPathS)
+                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+            if (candidate.U[index] < -ZeroTolerance)
+                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
+
+            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
+                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
+            previousPathS = candidate.S[index];
+        }
+
+        TrajectorySample terminal = samples[samples.Count - 1];
+        double holdElapsed = 0d;
+        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
+        {
+            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
+            samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true));
+        }
+
+        Samples = new ReadOnlyCollection<TrajectorySample>(samples);
+    }
+
+    public IReadOnlyList<TrajectorySample> Samples { get; }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class TrajectorySample
+{
+    public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk,
+        bool isHoldSample)
+    {
+        TimeFromStart = timeFromStart;
+        PathS = pathS;
+        ProgressSpeed = progressSpeed;
+        Acceleration = acceleration;
+        Jerk = jerk;
+        IsHoldSample = isHoldSample;
+    }
+
+    public double TimeFromStart { get; }
+    public double PathS { get; }
+    public double ProgressSpeed { get; }
+    public double Acceleration { get; }
+    public double Jerk { get; }
+    public bool IsHoldSample { get; }
+}
@@ -11,7 +11,7 @@ internal static class Program
            args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
            args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
            args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
-            args[0] != "longitudinal-real-osqp-probe"))
+            args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory"))
        {
            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
            return 2;
@@ -88,6 +88,11 @@ internal static class Program
                MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.RunRealOsqp();
                Console.WriteLine("PASS longitudinal-real-osqp");
            }
+            if (args[0] == "trajectory")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
+                Console.WriteLine("PASS trajectory");
+            }
            return 0;
        }
        catch (Exception exception)
@@ -0,0 +1,128 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class TrajectoryChecks
+{
+    public static void Run()
+    {
+        VerifiesForwardFieldsExactTerminalAndHold();
+        VerifiesReverseTravelVelocityAndUnwrappedYaw();
+        VerifiesPublishedListsAreImmutable();
+    }
+
+    private static void VerifiesForwardFieldsExactTerminalAndHold()
+    {
+        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Forward, 0d, Math.PI / 2d),
+            CreateLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal));
+
+        VerifyKinematicFields(trajectory, TravelDirection.Forward, "forward");
+        VerifyTerminalAndHold(trajectory, EmBoundaryType.Goal, "forward");
+    }
+
+    private static void VerifiesReverseTravelVelocityAndUnwrappedYaw()
+    {
+        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Reverse, 3.10d, -3.10d),
+            CreateLongitudinalResult(),
+            CreateMetadata(TravelDirection.Reverse, EmTerminalType.GearSwitch));
+
+        VerifyKinematicFields(trajectory, TravelDirection.Reverse, "reverse");
+        VerifyTerminalAndHold(trajectory, EmBoundaryType.GearSwitchApproach, "reverse");
+        EmTrajectoryPoint moving = trajectory.Points[1];
+        Verification.True(moving.SignedLongitudinalVelocity < 0d, "reverse signed velocity is negative");
+        Verification.True(moving.VelocityX * Math.Cos(moving.Yaw) + moving.VelocityY * Math.Sin(moving.Yaw) < 0d,
+            "reverse world velocity points opposite the vehicle yaw");
+        Verification.True(Math.Abs(Math.Abs(moving.Yaw) - Math.PI) < 0.1d,
+            "reverse yaw interpolation unwraps across the pi boundary");
+    }
+
+    private static void VerifiesPublishedListsAreImmutable()
+    {
+        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop));
+        Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
+            "trajectory public point list is immutable");
+    }
+
+    private static void VerifyKinematicFields(EmTrajectory trajectory, TravelDirection direction, string name)
+    {
+        double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
+        double previousTime = double.NegativeInfinity;
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = trajectory.Points[index];
+            Verification.NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed,
+                name + " speed field " + index);
+            Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX,
+                name + " velocity X field " + index);
+            Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY,
+                name + " velocity Y field " + index);
+            Verification.NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate,
+                name + " yaw-rate field " + index);
+            if (index < 4)
+                Verification.NearlyEqual(directionSign * CreateLongitudinalResult().Candidate.U[index],
+                    point.SignedLongitudinalVelocity, name + " signed speed field " + index);
+            Verification.True(point.TimeFromStart > previousTime, name + " time strictly increases " + index);
+            Verification.True(point.PathS >= previousPathS, name + " PathS never decreases " + index);
+            previousTime = point.TimeFromStart;
+            previousPathS = point.PathS;
+        }
+    }
+
+    private static void VerifyTerminalAndHold(EmTrajectory trajectory, EmBoundaryType terminalBoundary, string name)
+    {
+        const int terminalIndex = 3;
+        EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
+        Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type");
+        Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed");
+        Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate");
+        Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time");
+        Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
+
+        for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint hold = trajectory.Points[index];
+            Verification.NearlyEqual(terminal.TimeFromStart + (index - terminalIndex) * 0.05d,
+                hold.TimeFromStart, name + " hold timing " + index);
+            Verification.NearlyEqual(terminal.X, hold.X, name + " hold X " + index);
+            Verification.NearlyEqual(terminal.Y, hold.Y, name + " hold Y " + index);
+            Verification.NearlyEqual(terminal.Yaw, hold.Yaw, name + " hold yaw " + index);
+            Verification.NearlyEqual(0d, hold.SignedLongitudinalVelocity, name + " hold signed speed " + index);
+            Verification.NearlyEqual(0d, hold.YawRate, name + " hold yaw rate " + index);
+        }
+    }
+
+    private static LateralPath CreatePath(TravelDirection direction, double firstYaw, double lastYaw)
+    {
+        return new LateralPath(new[]
+        {
+            new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, firstYaw, 0d, 0.5d, 0d),
+            new LateralPathPoint(1d, 0.12d, 0d, 0d, 0d, 0d, 0.12d, 0d, lastYaw, 0d, 0.5d, 0d),
+        }, true);
+    }
+
+    private static LongitudinalPlanningResult CreateLongitudinalResult()
+    {
+        var candidate = new LongitudinalCandidate(
+            new[] { 0d, 0.05d, 0.10d, 0.137d },
+            new[] { 0d, 0.04d, 0.08d, 0.12d },
+            new[] { 0.8d, 0.8d, 0.8d, 0d },
+            new[] { 0d, 0d, 0d, 0d },
+            new[] { 0d, 0d, 0d });
+        return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
+    }
+
+    private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType)
+    {
+        return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
+            "reference", 4L, string.Empty, 2, direction, terminalType);
+    }
+}