From 0d4e1aeb478762876b6c197eebe8dcef98561053 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= <lby115@126.com>
Date: Fri, 7 Aug 2026 16:51:11 +0800
Subject: [PATCH] fix: treat near-zero start as stopped in full-direction
 schedule

---
 .../FullDirectionSegmentScheduleBuilder.cs    | 11 ++++++---
 .../LongitudinalIntegrationChecks.cs          | 24 +++++++++++++++++++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs
index 209f36c..266b479 100644
--- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs
@@ -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 _))
         {
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
index bb537ec..395ce2a 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
@@ -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();