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

fix: treat near-zero start as stopped in full-direction schedule

Browse Source
This commit is contained in:
梁薄云
2026-08-07 16:51:11 +08:00
parent 2d406359d0
commit 0d4e1aeb47
2 changed files with 32 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs
+8 -3
View File
@@ -46,7 +46,11 @@ public sealed class FullDirectionSegmentScheduleBuilder
int stationCount = speedLimit.PathS.Count;
var speeds = new double[stationCount];
double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond);
speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired,
double effectiveInitialSpeed = initialProgressSpeedMetersPerSecond <=
longitudinal.StopSpeedToleranceMetersPerSecond
? 0d
: initialProgressSpeedMetersPerSecond;
speeds[0] = Math.Min(effectiveInitialSpeed, Math.Min(desired,
speedLimit.MaximumSpeedMetersPerSecond[0]));
for (int index = 1; index < stationCount; index++)
{
@@ -100,7 +104,7 @@ public sealed class FullDirectionSegmentScheduleBuilder
referenceSpeeds[referenceSpeeds.Count - 1] = 0d;
pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS;
EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal);
EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond,
EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, effectiveInitialSpeed,
initialAccelerationMetersPerSecondSquared, longitudinal);
if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d)
{
@@ -139,7 +143,8 @@ public sealed class FullDirectionSegmentScheduleBuilder
private static void EnsureMinimumExactStopDuration(IList<double> times, double stopBoundaryPathS,
double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration)
{
if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
if (initialSpeed <= configuration.StopSpeedToleranceMetersPerSecond ||
!JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
configuration.MaximumDecelerationMetersPerSecondSquared,
configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _))
{
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+24
View File
@@ -17,6 +17,7 @@ internal static class LongitudinalIntegrationChecks
VerifiesFullDirectionStaticStartMakesProgress();
VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed();
VerifiesFullDirectionNearZeroScheduleDurationIsReasonable();
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -224,6 +225,29 @@ internal static class LongitudinalIntegrationChecks
"curved near-zero start stops at the terminal boundary");
}
private static void VerifiesFullDirectionNearZeroScheduleDurationIsReasonable()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
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, "near-zero duration 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, "near-zero duration schedule: " + failureReason);
double duration = schedule.KnotTimes[schedule.KnotTimes.Count - 1];
Verification.True(duration < 120d,
"near-zero schedule duration is normal: " + duration.ToString("R", System.Globalization.CultureInfo.InvariantCulture));
}
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();