feat: publish rolling trajectories without stop tails

This commit is contained in:
梁薄云
2026-08-05 20:00:11 +08:00
parent 72592f0cae
commit 4159ae0bc1
9 changed files with 196 additions and 50 deletions
@@ -39,7 +39,8 @@ public sealed class EmTrajectoryAssembler
throw new ArgumentNullException(nameof(metadata));
var interpolator = new LateralPathInterpolator(path);
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds);
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds,
metadata.LongitudinalMode);
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;
@@ -51,7 +52,8 @@ public sealed class EmTrajectoryAssembler
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;
bool isTerminalAnchor = metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary &&
index == schedule.TerminalAnchorSampleIndex;
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,