feat: publish rolling trajectories without stop tails
This commit is contained in:
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user