diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 2848423180..cc2d7f27c5 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/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), diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index fe1b9f5a58..664b310cfd 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/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)