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