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

fix: stabilize stopped-start EM planning and add txt diagnostics

Browse Source
This commit is contained in:
梁薄云
2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+10 -14
View File
@@ -194,31 +194,27 @@ internal static class TrajectoryChecks
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),
EmTrajectoryValidationResult exactPosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, terminal.Yaw),
EmBoundaryType.Goal);
Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted");
Verification.True(exactPosition.IsValid, "exact terminal position 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),
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.001d, terminal.Y, terminal.Yaw),
EmBoundaryType.Goal);
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
"3.1-centimetre terminal position error is rejected");
"1-millimetre terminal position error is rejected");
double degrees = Math.PI / 180d;
EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees));
EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: Math.PI));
EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees),
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -Math.PI),
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");
Verification.True(wrapped.IsValid, "PI and -PI use normalized yaw error");
EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees),
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -0.1d * degrees),
EmBoundaryType.Goal);
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
"5.1-degree terminal yaw error is rejected");
"0.1-degree terminal yaw error is rejected");
EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),