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

feat: validate EM terminal world pose

Browse Source
This commit is contained in:
梁薄云
2026-08-07 08:01:18 +08:00
parent 0bba8d7e61
commit e844fe00a8
6 changed files with 249 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+62 -3
View File
@@ -17,6 +17,7 @@ internal static class TrajectoryChecks
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
VerifiesExactStopHoldHasNoJerkDiscontinuity();
VerifiesWorldSpacePublicationMutationsAreRejected();
VerifiesTerminalPoseAndSampleLimitPublicationGates();
VerifiesReverseSpeedLimitUsesReverseConfiguration();
}
@@ -184,6 +185,64 @@ internal static class TrajectoryChecks
Verification.Equal(4, beyondSegment.PointIndex, "segment-boundary first point");
}
private static void VerifiesTerminalPoseAndSampleLimitPublicationGates()
{
ValidationContext context = CreateValidationContext();
EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
EmTrajectoryPoint terminal = valid.Points[terminalIndex];
var validator = new EmTrajectoryValidator();
EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw),
EmBoundaryType.Goal);
Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted");
EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw),
EmBoundaryType.Goal);
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
"3.1-centimetre terminal position error is rejected");
double degrees = Math.PI / 180d;
EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees));
EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees),
EmBoundaryType.Goal);
Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error");
EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees),
EmBoundaryType.Goal);
Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted");
EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees),
EmBoundaryType.Goal);
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
"5.1-degree terminal yaw error is rejected");
EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation));
EmTrajectoryValidationResult rollingResult = validator.Validate(rolling, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.08d, new Pose2D(99d, 99d, Math.PI), EmBoundaryType.RollingSafetyStop);
Verification.True(rollingResult.IsValid, "rolling-safety window end does not use the terminal-pose gate");
EmPlannerConfiguration limitedConfiguration = EmPlannerConfiguration.CreateDefault();
limitedConfiguration.Scheduling.MaximumPublishedSampleCount = 8;
EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(limitedConfiguration).TryAssemble(
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal), out EmTrajectory limitedTrajectory,
out string assemblyFailure);
Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, assemblyStatus,
"sample schedule one above the limit is rejected");
Verification.True(limitedTrajectory == null, "sample-limit rejection does not assemble a partial trajectory");
Verification.True(assemblyFailure.IndexOf("MaximumPublishedSampleCount", StringComparison.Ordinal) >= 0,
"sample-limit rejection preserves a diagnostic");
Verification.True(assemblyFailure.IndexOf("required-at-least=9", StringComparison.Ordinal) >= 0,
"sample-limit fixture exceeds the cap by exactly one sample");
}
private static void VerifiesReverseSpeedLimitUsesReverseConfiguration()
{
ValidationContext context = CreateValidationContext();
@@ -284,11 +343,11 @@ internal static class TrajectoryChecks
return Replace(trajectory, index, replacement);
}
private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? timeFromStart = null,
double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? y = null, double? yaw = null,
double? timeFromStart = null, double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
EmBoundaryType? boundaryType = null)
{
return new EmTrajectoryPoint(x ?? point.X, point.Y, point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity,
return new EmTrajectoryPoint(x ?? point.X, y ?? point.Y, yaw ?? point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity,
timeFromStart ?? point.TimeFromStart, vehicleCurvature ?? point.VehicleCurvature, point.SegmentIndex,
pathS ?? point.SegmentLocalS, pathS ?? point.PathS, point.Direction, boundaryType ?? point.BoundaryType,
0d, 0d);