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/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;