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

test: reproduce EM PathS resampling regression

Browse Source
This commit is contained in:
梁薄云
2026-08-10 14:22:38 +08:00
parent feae644659
commit 8a1ba2a6d5
1 changed files with 64 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+64 -2
View File
@@ -20,6 +20,8 @@ internal static class TrajectoryChecks
VerifiesWorldSpacePublicationMutationsAreRejected();
VerifiesTerminalPoseAndSampleLimitPublicationGates();
VerifiesReverseSpeedLimitUsesReverseConfiguration();
VerifiesFullScopeNormalizesToleranceSizedPathSResiduals();
VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics();
}
private static void VerifiesForwardFieldsExactTerminalAndHold()
@@ -479,11 +481,71 @@ internal static class TrajectoryChecks
new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
}
private static void VerifiesFullScopeNormalizesToleranceSizedPathSResiduals()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
var candidate = new LongitudinalCandidate(
new[] { 0d, 0.1d, 0.2d, 0.3d },
new[] { 0d, 0.05d, 0.05d - 0.5d * configuration.Validation.KinematicTolerance, 0.10d },
new[] { 0.5d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d });
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d),
new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
for (int index = 1; index < trajectory.Points.Count; index++)
{
Verification.True(trajectory.Points[index].PathS >= trajectory.Points[index - 1].PathS,
"full-scope numerical residual is normalized at sample " + index);
}
Verification.True(trajectory.Points[trajectory.Points.Count - 1].PathS <= candidate.S[candidate.S.Count - 1],
"full-scope publication remains within terminal PathS");
}
private static void VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
var candidate = new LongitudinalCandidate(
new[] { 0d, 0.1d, 0.2d, 0.3d },
new[] { 0d, 0.05d, 0.04d, 0.10d },
new[] { 0.5d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d, 0d },
new[] { 0d, 0d, 0d });
try
{
new EmTrajectoryAssembler(configuration).Assemble(
CreatePath(TravelDirection.Forward, 0d, 0d),
new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
throw new InvalidOperationException("Material PathS regression was accepted.");
}
catch (ArgumentException exception)
{
Verification.True(exception.Message.Contains("sampleIndex=2"),
"PathS diagnostic contains sample index");
Verification.True(exception.Message.Contains("previousPathS="),
"PathS diagnostic contains previous value");
Verification.True(exception.Message.Contains("candidatePathS="),
"PathS diagnostic contains candidate value");
Verification.True(exception.Message.Contains("difference="),
"PathS diagnostic contains difference");
}
}
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary,
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
{
return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
"reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode,
EmPlanningScope.RollingHorizon);
planningScope);
}
}