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

feat: derive adaptive full-segment ST schedule

Browse Source
This commit is contained in:
梁薄云
2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+94
View File
@@ -13,6 +13,8 @@ internal static class LongitudinalIntegrationChecks
public static void Run()
{
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
@@ -58,6 +60,82 @@ internal static class LongitudinalIntegrationChecks
"rolling ST keeps nonzero terminal speed");
}
private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.TimeHorizonSeconds = 10d;
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(1d, 1d, 0d),
Point(2d, 2d, 0d),
}, true);
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
out LongitudinalKnotSchedule coarsePublication, out failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);
EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
out LongitudinalKnotSchedule densePublication, out failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
"publication cadence does not determine full-segment optimization knot count");
var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
new[] { 0.80d, 0d });
var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
DateTimeOffset now = DateTimeOffset.UtcNow;
var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
EmPlanningScope.FullDirectionSegment);
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
publicationResult, metadata);
Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
"halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
}
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(1d, 0.0075d, 0d),
}, true);
var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
DateTimeOffset now = DateTimeOffset.UtcNow;
var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
EmPlanningScope.FullDirectionSegment);
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);
Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
"full-scope publication reuses its candidate hold instead of appending a second hold");
Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
"full-scope publication emits the candidate's single nonzero terminal hold");
}
private static void VerifiesExactStopIncludesAStabilizationTail()
{
EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
@@ -97,6 +175,22 @@ internal static class LongitudinalIntegrationChecks
}
}
private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
{
double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
int count = 0;
for (int index = 0; index < trajectory.Points.Count; index++)
{
EmTrajectoryPoint point = trajectory.Points[index];
if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
{
count++;
}
}
return count;
}
public static void RunRealOsqp()
{
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())