Force Stop fix

openpilot/starpilot/longitudinal/force_stop.py

@@ -193,7 +193,7 @@ class ForceStop:
     else:
       self.timer = max(0, self.timer - DT_MDL * .25)
     force = self.timer >= STOP_COMMIT_TIME_S or self.forcing and not frame.standstill or held
-    light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and
+    light_cleared = (not held and self.forcing and self.from_light and not frame.standstill and not self.sign_confirmed and
                      available and not frame.light and not frame.sign and go_scene)
     low_speed_hold = (light_cleared and tune.low_speed_hold_mps is not None and self.entry_speed is not None and
                       speed <= tune.low_speed_hold_mps and speed < self.entry_speed - .25)
@@ -234,8 +234,6 @@ class ForceStop:
         if frame.sign and available:
           if horizon < SIGN_MODEL_AGREE_M:
             self.distance = min(self.distance, SIGN_SEED_M)
-          if horizon < SIGN_SEED_M:
-            self.distance = horizon
         effective = self.distance + offset + tune.distance_bias_m
         decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
         ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
@@ -251,10 +249,12 @@ class ForceStop:
         if effective > tune.handoff_m:
           ceiling = math.sqrt(2 * MODEL_APPROACH_DECEL * (effective - tune.handoff_m))
     self.previously_enabled = frame.enabled
-    stop_length = self.distance if self.forcing and self.distance > tune.handoff_m else approach
+    stop_length = self.distance if self.forcing else approach
     # The MPC obstacle includes its standing-distance term; adding it at the
     # native boundary avoids moving the actual stop line by that buffer.
-    obstacle = stop_length * .93 + offset + tune.distance_bias_m if stop_length > tune.handoff_m else None
+    # The offset moves the handoff with the ceiling; otherwise a positive offset creeps with no stop target.
+    obstacle = (stop_length * .93 + offset + tune.distance_bias_m
+                if stop_length > 0 and stop_length + offset > tune.handoff_m else None)
     return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5),
                     self.distance if self.forcing else 0.0, approach,
                     tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden),

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -229,6 +229,28 @@ class ForceStopTests(unittest.TestCase):
           plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune)
           self.assertFalse(plan.forcing)
 
+  def test_latched_sign_survives_light_and_sign_dropout(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune)
+    self.assertTrue(self.owner.sign_confirmed)
+    for tick in range(15, 45):
+      plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=190., light=False), self.tune)
+      self.assertTrue(plan.forcing)
+
+  def test_sign_seed_never_moves_the_stop_farther(self):
+    for tick in range(50):
+      previous = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., sign=True), self.tune)
+    self.assertLess(previous.tracked_distance_m, 20.)
+    plan = self.owner.step(frame(50, speed_mps=8., horizon_m=20., sign=True), self.tune)
+    self.assertLessEqual(plan.tracked_distance_m, previous.tracked_distance_m)
+
+  def test_positive_offset_keeps_stop_target_until_native_stop(self):
+    for tick in range(40):
+      plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, 8)
+      if plan.forcing:
+        self.assertTrue(plan.should_stop or plan.obstacle_m is not None)
+    self.assertTrue(plan.should_stop)
+
   def test_odometry_ceiling_bounds_reopened_model(self):
     for tick in range(15):
       self.owner.step(frame(tick), self.tune)