First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: publish rolling trajectories without stop tails

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs
+6 -1
View File
@@ -15,7 +15,8 @@ public sealed class EmTrajectoryMetadata
string previousTrajectoryId,
int segmentIndex,
TravelDirection direction,
EmTerminalType terminalType)
EmTerminalType terminalType,
EmLongitudinalMode longitudinalMode)
{
if (string.IsNullOrWhiteSpace(trajectoryId))
throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId));
@@ -31,6 +32,8 @@ public sealed class EmTrajectoryMetadata
throw new ArgumentOutOfRangeException(nameof(direction));
if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
throw new ArgumentOutOfRangeException(nameof(terminalType));
if (!Enum.IsDefined(typeof(EmLongitudinalMode), longitudinalMode))
throw new ArgumentOutOfRangeException(nameof(longitudinalMode));
TrajectoryId = trajectoryId;
GeneratedAtUtc = generatedAtUtc;
@@ -42,6 +45,7 @@ public sealed class EmTrajectoryMetadata
SegmentIndex = segmentIndex;
Direction = direction;
TerminalType = terminalType;
LongitudinalMode = longitudinalMode;
}
public string TrajectoryId { get; }
@@ -54,4 +58,5 @@ public sealed class EmTrajectoryMetadata
public int SegmentIndex { get; }
public TravelDirection Direction { get; }
public EmTerminalType TerminalType { get; }
public EmLongitudinalMode LongitudinalMode { get; }
}
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
+2 -2
View File
@@ -97,12 +97,12 @@ public sealed class EmPlanningService : IEmPlanningService
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
segment.SegmentIndex, segment.Direction, horizon.TerminalType);
segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode);
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
EmitDebug(request, "trajectory assembly succeeded");
EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.TerminalPathS,
request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
slice.TerminalBoundary.BoundaryType);
if (!publication.IsValid)
{
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
+4 -2
View File
@@ -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,
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs
+26 -3
View File
@@ -8,7 +8,8 @@ internal sealed class TrajectorySampleSchedule
{
private const double ZeroTolerance = 1e-12d;
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
EmLongitudinalMode mode)
{
if (candidate == null)
throw new ArgumentNullException(nameof(candidate));
@@ -16,8 +17,8 @@ internal sealed class TrajectorySampleSchedule
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));
if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
throw new ArgumentOutOfRangeException(nameof(mode));
var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
double previousPathS = double.NegativeInfinity;
@@ -33,6 +34,27 @@ internal sealed class TrajectorySampleSchedule
previousPathS = candidate.S[index];
}
if (mode != EmLongitudinalMode.ExactStopAtBoundary)
{
Samples = new ReadOnlyCollection<TrajectorySample>(samples);
TerminalAnchorSampleIndex = -1;
return;
}
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
outputTimeStepSeconds);
double stopPathS = candidate.S[stabilizationStart];
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
(index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
{
throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
}
}
TerminalAnchorSampleIndex = stabilizationStart;
TrajectorySample terminal = samples[samples.Count - 1];
double holdElapsed = 0d;
while (holdElapsed < holdDurationSeconds - ZeroTolerance)
@@ -45,6 +67,7 @@ internal sealed class TrajectorySampleSchedule
}
public IReadOnlyList<TrajectorySample> Samples { get; }
public int TerminalAnchorSampleIndex { get; }
private static bool IsFinite(double value)
{
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs
+40 -17
View File
@@ -16,6 +16,7 @@ public enum EmTrajectoryValidationFailure
SegmentBoundaryExceeded,
MissingTerminalAnchor,
TerminalSpeedNotZero,
TerminalAccelerationNotZero,
TerminalYawRateNotZero,
DirectionMismatch,
DirectionSignMismatch,
@@ -77,11 +78,11 @@ public sealed class EmTrajectoryValidator
}
public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
EmPlannerConfiguration configuration, int segmentIndex, double terminalPathS, EmBoundaryType terminalBoundary)
EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
{
if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
!IsFinite(terminalPathS) || terminalPathS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
!IsFinite(pathUpperBoundS) || pathUpperBoundS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
!TryReadLimits(configuration, vehicle, trajectory.Metadata.Direction, out ValidationLimits limits))
{
return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
@@ -93,8 +94,8 @@ public sealed class EmTrajectoryValidator
EmTrajectoryPoint point = trajectory.Points[index];
if (point == null || !HasOnlyFiniteValues(point))
return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > terminalPathS + limits.SpatialTolerance ||
point.PathS > terminalPathS + limits.SpatialTolerance)
if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
point.PathS > pathUpperBoundS + limits.SpatialTolerance)
{
return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,
"Trajectory point lies outside the current direction segment.");
@@ -114,21 +115,43 @@ public sealed class EmTrajectoryValidator
}
}
int terminalIndex = FindTerminalAnchor(trajectory, terminalPathS, terminalBoundary, limits.SpatialTolerance);
if (terminalIndex < 0)
int terminalIndex = -1;
if (trajectory.Metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary)
{
return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
FindFirstTerminalPathIndex(trajectory, terminalPathS, limits.SpatialTolerance),
"Trajectory does not contain the exact terminal boundary anchor.");
}
terminalIndex = FindTerminalAnchor(trajectory, pathUpperBoundS, terminalBoundary, limits.SpatialTolerance);
if (terminalIndex < 0)
{
return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
FindFirstTerminalPathIndex(trajectory, pathUpperBoundS, limits.SpatialTolerance),
"Trajectory does not contain the exact terminal boundary anchor.");
}
EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex,
"Terminal signed speed must be zero.");
if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
"Terminal yaw rate must be zero.");
EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex,
"Terminal signed speed must be zero.");
if (Math.Abs(terminal.LongitudinalAcceleration) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex,
"Terminal stored acceleration must be zero.");
if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
"Terminal yaw rate must be zero.");
for (int index = terminalIndex; index < trajectory.Points.Count; index++)
{
EmTrajectoryPoint tail = trajectory.Points[index];
if (Math.Abs(tail.PathS - terminal.PathS) > limits.SpatialTolerance)
{
return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor, index,
"Exact-stop tail must remain at the terminal PathS.");
}
if (Math.Abs(tail.SignedLongitudinalVelocity) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, index,
"Exact-stop tail signed speed must remain zero.");
if (Math.Abs(tail.LongitudinalAcceleration) > limits.KinematicTolerance)
return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, index,
"Exact-stop tail stored acceleration must remain zero.");
}
}
double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
double previousAcceleration = 0d;