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

fix: generate deterministic S-curve static-start ST seeds

Browse Source
This commit is contained in:
梁薄云
2026-08-07 16:33:12 +08:00
parent d378ad6257
commit 2d406359d0
2 changed files with 117 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+37
View File
@@ -16,6 +16,7 @@ internal static class LongitudinalIntegrationChecks
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
VerifiesFullDirectionStaticStartMakesProgress();
VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed();
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -187,6 +188,42 @@ internal static class LongitudinalIntegrationChecks
"near-zero start stops at the terminal boundary");
}
private static void VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.SolverTimeoutSeconds = 5d;
configuration.Solver.MaximumOsqpIterations = 100000;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(1d, 2.25d, 0.1d),
Point(2d, 4.5d, 0d),
}, true);
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero envelope: " + failureReason);
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
out LongitudinalKnotSchedule schedule, out failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero schedule: " + failureReason);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.0001d, 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,
"curved near-zero full-direction start succeeds: " + result.FailureReason);
LongitudinalCandidate candidate = result.Candidate ??
throw new InvalidOperationException("Curved near-zero full-direction candidate was missing.");
Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
"curved near-zero start reaches the terminal boundary");
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
"curved near-zero start stops at the terminal boundary");
}
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();