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/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+103 -19
View File
@@ -14,6 +14,8 @@ internal static class TrajectoryChecks
VerifiesForwardFieldsExactTerminalAndHold();
VerifiesReverseTravelVelocityAndUnwrappedYaw();
VerifiesPublishedListsAreImmutable();
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
VerifiesExactStopHoldHasNoJerkDiscontinuity();
VerifiesWorldSpacePublicationMutationsAreRejected();
VerifiesReverseSpeedLimitUsesReverseConfiguration();
}
@@ -50,11 +52,67 @@ internal static class TrajectoryChecks
{
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop));
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation));
Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
"trajectory public point list is immutable");
}
private static void VerifiesRollingTrajectoryHasNoSyntheticStopTail()
{
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation));
Verification.Equal(21, trajectory.Points.Count, "rolling trajectory keeps only ST knots");
EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
Verification.Equal(EmBoundaryType.None, terminal.BoundaryType,
"rolling horizon end is not a boundary anchor");
Verification.True(terminal.SignedLongitudinalVelocity > 0d,
"rolling terminal speed stays nonzero");
Verification.Equal(EmLongitudinalMode.RollingContinuation, trajectory.Metadata.LongitudinalMode,
"rolling longitudinal mode is published");
}
private static void VerifiesExactStopHoldHasNoJerkDiscontinuity()
{
ValidationContext context = CreateValidationContext();
EmTrajectory trajectory = CreateValidationTrajectory(TravelDirection.Forward);
EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, context.EmptyMap,
context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
Verification.True(result.IsValid, "exact stop with internal tail and external hold is publishable: " + result.Message);
int anchorIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
EmTrajectoryPoint previousMoving = trajectory.Points[anchorIndex - 1];
EmTrajectoryPoint anchor = trajectory.Points[anchorIndex];
EmTrajectoryPoint stabilization = trajectory.Points[anchorIndex + 1];
EmTrajectoryPoint firstExternalHold = trajectory.Points[anchorIndex + 2];
Verification.Equal(EmBoundaryType.Goal, anchor.BoundaryType, "exact stop marks only the real boundary anchor");
Verification.Equal(EmBoundaryType.None, stabilization.BoundaryType,
"QP stabilization point is not a duplicate boundary anchor");
Verification.Equal(EmBoundaryType.None, firstExternalHold.BoundaryType,
"external hold is not a duplicate boundary anchor");
double terminalFiniteDifferenceAcceleration =
(anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
(anchor.TimeFromStart - previousMoving.TimeFromStart);
double stabilizationAcceleration =
(stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
(stabilization.TimeFromStart - anchor.TimeFromStart);
double externalHoldAcceleration =
(firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
double jerkIntoStabilization = (stabilizationAcceleration - terminalFiniteDifferenceAcceleration) /
(stabilization.TimeFromStart - anchor.TimeFromStart);
double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
"jerk into QP stabilization stays within the limit");
Verification.NearlyEqual(0d, jerkIntoExternalHold, "jerk into the external hold is zero");
}
private static void VerifiesWorldSpacePublicationMutationsAreRejected()
{
ValidationContext context = CreateValidationContext();
@@ -104,11 +162,14 @@ internal static class TrajectoryChecks
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 0.75d)),
EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, 1, "curvature-rate limit");
const int terminalIndex = 8;
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], boundaryType: EmBoundaryType.None)),
EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex, "missing exact terminal anchor");
EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex - 1, "missing exact terminal anchor");
AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], signedSpeed: 0.01d)),
EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex, "terminal speed");
AssertRejected(context, CorruptDouble(valid, terminalIndex, "LongitudinalAcceleration", 0.01d),
EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex, "terminal acceleration");
AssertRejected(context, CorruptDouble(valid, terminalIndex, "YawRate", 0.01d),
EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex, "terminal yaw rate");
@@ -186,13 +247,7 @@ internal static class TrajectoryChecks
private static EmTrajectory CreateValidationTrajectory(TravelDirection direction)
{
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d };
double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
0.00546875d, 0.0055d };
double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d };
double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d };
double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d };
var candidate = new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
LongitudinalCandidate candidate = CreateValidationCandidate();
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
var path = new LateralPath(new[]
{
@@ -202,6 +257,17 @@ internal static class TrajectoryChecks
return new EmTrajectoryAssembler().Assemble(path, result, CreateMetadata(direction, EmTerminalType.Goal));
}
private static LongitudinalCandidate CreateValidationCandidate()
{
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d, 0.45d, 0.50d };
double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
0.00546875d, 0.0055d, 0.0055d, 0.0055d };
double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d, 0d, 0d };
double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d, 0d, 0d };
double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0d, 0d };
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
}
private static EmTrajectory Replace(EmTrajectory trajectory, int index, EmTrajectoryPoint replacement)
{
var points = new List<EmTrajectoryPoint>(trajectory.Points);
@@ -280,8 +346,8 @@ internal static class TrajectoryChecks
Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type");
Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed");
Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate");
Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time");
Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
Verification.NearlyEqual(0.15d, terminal.TimeFromStart, name + " exact terminal stabilization time");
Verification.Equal(terminalIndex + 6, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++)
{
@@ -308,17 +374,35 @@ internal static class TrajectoryChecks
private static LongitudinalPlanningResult CreateLongitudinalResult()
{
var candidate = new LongitudinalCandidate(
new[] { 0d, 0.05d, 0.10d, 0.137d },
new[] { 0d, 0.04d, 0.08d, 0.12d },
new[] { 0.8d, 0.8d, 0.8d, 0d },
new[] { 0d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d });
new[] { 0d, 0.05d, 0.10d, 0.15d, 0.20d },
new[] { 0d, 0.04d, 0.08d, 0.12d, 0.12d },
new[] { 0.8d, 0.8d, 0.2d, 0d, 0d },
new[] { 0d, 0d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d, 0d });
return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
}
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType)
private static LongitudinalPlanningResult CreateRollingLongitudinalResult()
{
var times = new double[21];
var pathS = new double[21];
var speed = new double[21];
var acceleration = new double[21];
var jerk = new double[20];
for (int index = 0; index < times.Length; index++)
{
times[index] = index * 0.1d;
pathS[index] = index * 0.004d;
speed[index] = 0.04d;
}
return new LongitudinalPlanningResult(EmPlanningStatus.Success,
new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
}
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
{
return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
"reference", 4L, string.Empty, 2, direction, terminalType);
"reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode);
}
}