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/FoundationChecks.cs
+5 -3
View File
@@ -101,6 +101,8 @@ internal static class FoundationChecks
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Validation.KinematicTolerance,
"kinematic tolerance");
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");
@@ -179,9 +181,9 @@ internal static class FoundationChecks
"optimization knot cap");
EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount,
"publication sample cap");
EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance");
EMPlannerVerificationHost.Verification.NearlyEqual(5d * Math.PI / 180d,
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance");
EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress,
"no-progress status exists");
@@ -200,7 +202,7 @@ internal static class FoundationChecks
copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed");
EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount,
"copy keeps knot cap");
EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance");
EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault();
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+37
View File
@@ -15,6 +15,7 @@ internal static class LongitudinalIntegrationChecks
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
VerifiesFullDirectionStaticStartMakesProgress();
VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -150,6 +151,42 @@ internal static class LongitudinalIntegrationChecks
"full segment stops at the terminal boundary");
}
private static void VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.SolverTimeoutSeconds = 5d;
configuration.Solver.MaximumOsqpIterations = 100000;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(2.5d, 2.5d, 0d),
Point(5d, 5d, 0d),
}, true);
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.01d,
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "near-zero envelope: " + failureReason);
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.01d, 0d,
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
out LongitudinalKnotSchedule schedule, out failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "near-zero schedule: " + failureReason);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.01d, 0d,
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
CancellationToken.None);
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
"near-zero full-direction start succeeds: " + result.FailureReason);
LongitudinalCandidate candidate = result.Candidate ??
throw new InvalidOperationException("Near-zero full-direction candidate was missing.");
Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
"near-zero start reaches the terminal boundary");
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
"near-zero start stops at the terminal boundary");
}
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+3 -3
View File
@@ -769,9 +769,9 @@ internal static class LongitudinalModelChecks
out speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract");
Verification.True(failureStatus != EmPlanningStatus.NoProgress,
"near-terminal gear-switch stop is not classified as no progress");
out failureStatus, out _), "near-terminal stationary full segment is rejected");
Verification.Equal(EmPlanningStatus.NoProgress, failureStatus,
"near-terminal gear-switch stop is classified as no progress");
}
private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
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(),
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
+44
View File
@@ -23,6 +23,7 @@ internal static class TrajectoryObservationChecks
{
TrajectoryObservationSettingsChecks.Run();
TrajectoryObservationSegmentChecks.Run();
VerifiesTrajectoryObservationReportWriterCreatesTxt();
VerifiesPainterPoseGeometryReplacesLargeArrow();
TrajectoryObservationVisualizationChecks.Run();
VerifiesObservationSourceHasNoActuatorCalls();
@@ -47,6 +48,7 @@ internal static class TrajectoryObservationChecks
VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
VerifiesPlanningDiagnosticsKeepRawFailureReason();
VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk();
VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
VerifiesLsPresentationUsesPathSOnHorizontalAxis();
VerifiesPainterStAxesUsePathSAndTimeUnits();
@@ -81,6 +83,29 @@ internal static class TrajectoryObservationChecks
}
}
private static void VerifiesTrajectoryObservationReportWriterCreatesTxt()
{
string reportDirectory = Path.Combine(Path.GetTempPath(),
"em-trajectory-report-" + Guid.NewGuid().ToString("N"));
try
{
var writer = new TrajectoryObservationReportWriter(17L, reportDirectory);
writer.Append("cycle=1 status=SolverTimedOut primal=140.918");
string reportPath = writer.Save();
Verification.True(File.Exists(reportPath), "trajectory observation txt report exists");
string text = File.ReadAllText(reportPath);
Verification.True(text.Contains("session=17"), "report includes session identity");
Verification.True(text.Contains("OBSERVE_ONLY"), "report records observation-only boundary");
Verification.True(text.Contains("SolverTimedOut"), "report includes cycle failure");
}
finally
{
if (Directory.Exists(reportDirectory))
Directory.Delete(reportDirectory, true);
}
}
private static void VerifiesObservationSourceUsesRequiredOperatorText()
{
string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
@@ -960,6 +985,25 @@ internal static class TrajectoryObservationChecks
}
}
private static void VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk()
{
DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
DirectionSegmentView segment = CreateStraightSegment();
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
string text = TrajectoryObservationDiagnostics.CreateCurveReport(charts, trajectory).Text;
foreach (string section in new[]
{
"LS_CURVE", "ST_CURVE", "SPEED_CURVE", "ACCELERATION_CURVE", "JERK_CURVE",
"referenceS(m)", "time(s)", "signedSpeed(m/s)", "acceleration(m/s2)", "jerk(m/s3)",
})
{
Verification.True(text.Contains(section), "curve report includes " + section);
}
}
private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
{
string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",