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