From 2a854471fcfe364aff0836fa6a620888f2ac35df Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 6 Nov 2026 05:18:08 -0600 Subject: [PATCH] Stabilize low-speed steering and force-stop behavior Consolidates commits authored by whoisdomi: - 64df004be19cd2a1a1a6a7eb5f802c2e83352473 No twitch test? - 3426045d1eec3f0990164b9d342cb308d28d19d5 twitch 2 - 2a4676dfca4987542811240f895100ece3cbef1c Force Stop fix - 233a386ab47122d9defab577ac83e3fe15e1fca9 twitch 2 fix - f831010290b44376ec0091ff6f84ce45a21b6e87 force stop edge case - 00d46b482d942d2ab253510e60512715735d9ae7 Force Stop: use Vision Stop distance - ec0221c16f16551cfc6259158a43d377567fcac2 twitch 3 - c86a4ac8d6637928d22798f4f4d765dae2a1d950 Force Stop Signs behind leads - d7d1fa02a131081300e3dec30d5842a3c2a69f7b Twitch 5 - 7e72a4bd8ecca0344442ca31c0a719275922e878 Force stop fix 2 Keep the model action curvature floor at 2.5 m/s, matching bluescreensonly4. --- openpilot/selfdrive/controls/controlsd.py | 15 +- .../starpilot/lateral/model_turn_assist.py | 62 ++++++-- .../tests/test_model_turn_assist_stop_hold.py | 129 +++++++++++++++++ .../starpilot/longitudinal/force_stop.py | 94 +++++++++++-- .../longitudinal/force_stop_runtime.py | 13 +- .../longitudinal/tests/test_force_stop.py | 132 ++++++++++++++++++ .../speed_limits/vision/stop_observation.py | 26 +++- .../vision/tests/test_stop_observation.py | 15 ++ 8 files changed, 449 insertions(+), 37 deletions(-) create mode 100644 openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 4f817c1983..f80954ff8d 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -478,22 +478,25 @@ class Controls: new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature assist = self.turn_assist_enabled() rolling = math.isfinite(CS.vEgo) and CS.vEgo > max(0.1 * CV.MPH_TO_MS, self.CP.minSteerSpeed) - if ( + assist_ready = ( assist - and rolling and self.sm.all_checks(['modelV2']) and CC.latActive - and not CS.standstill and not CS.steerFaultTemporary and not CS.steerFaultPermanent and CS.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) - ): + ) + if assist_ready and rolling and not CS.standstill: self.model_turn_assist.curvature = self.curvature - self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(self.model_turn_assist.twitch_guard_remaining, CS.vEgo, CS.standstill) + self.model_turn_assist.twitch_guard_armed = update_twitch_guard(self.model_turn_assist.twitch_guard_armed, CS.vEgo, CS.standstill) new_desired_curvature = self.model_turn_assist.update(CS, model_v2, CC, new_desired_curvature) + elif assist_ready: + self.model_turn_assist.curvature = self.curvature + new_desired_curvature = self.model_turn_assist.update_stopped(CS, new_desired_curvature) + self.model_turn_assist.twitch_guard_armed = update_twitch_guard(self.model_turn_assist.twitch_guard_armed, CS.vEgo, CS.standstill) elif getattr(self, 'model_turn_assist', None) is not None: self.model_turn_assist.reset() - self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(0.0, CS.vEgo, CS.standstill) + self.model_turn_assist.twitch_guard_armed = update_twitch_guard(False, CS.vEgo, CS.standstill) lane_base_curvature = new_desired_curvature self.lane_centering_applied = 0.0 self.lane_centering_inactive_reason = 'inactive' diff --git a/openpilot/starpilot/lateral/model_turn_assist.py b/openpilot/starpilot/lateral/model_turn_assist.py index 8620e1e180..5275c3344d 100644 --- a/openpilot/starpilot/lateral/model_turn_assist.py +++ b/openpilot/starpilot/lateral/model_turn_assist.py @@ -28,12 +28,17 @@ CURVATURE_HOLD_CONFIRM_MIN = 0.003 CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 TWITCH_GUARD_MAX_SPEED = 4.0 TWITCH_GUARD_FADE_SPEED = 3.0 -TWITCH_GUARD_DURATION = 1.5 TWITCH_GUARD_PLAN_RATIO = 4.0 TWITCH_GUARD_FLOOR = 0.002 TWITCH_GUARD_STRAIGHT_LO = 0.005 TWITCH_GUARD_STRAIGHT_HI = 0.014 TWITCH_GUARD_MIN_REACH = 12.0 +STOP_HOLD_MAX_SPEED = 2.5 +STOP_HOLD_DIST = 1.5 +STOP_HOLD_RELEASE_DIST = 3.0 +STOP_HOLD_MIN_DECEL = 0.3 +STOP_HOLD_MAX_CURVATURE = 0.01 +STOP_GUARD_DIST = 5.0 def _plan_circle_curvature(xs, ys, lookahead: float) -> float: @@ -118,12 +123,18 @@ def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float: return curvature + (math.copysign(limit, curvature) - curvature) * fade -def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float: - if not (math.isfinite(remaining) and math.isfinite(v_ego)): - return 0.0 +def update_twitch_guard(armed: bool, v_ego: float, standstill: bool) -> bool: + if not math.isfinite(v_ego): + return False if standstill or abs(v_ego) <= 0.3: - return TWITCH_GUARD_DURATION - return max(remaining - DT_CTRL, 0.0) + return True + return armed and v_ego < TWITCH_GUARD_MAX_SPEED + + +def stopping_distance(v_ego: float, a_ego: float, max_speed: float = STOP_HOLD_MAX_SPEED) -> float: + if not (math.isfinite(v_ego) and math.isfinite(a_ego)) or v_ego >= max_speed: + return math.inf + return v_ego ** 2 / (2.0 * max(-a_ego, 0.01)) TURN_LEAD_T = 1.3 TURN_LEAD_MIN_M = 4.0 TURN_LEAD_MAX_M = 14.0 @@ -149,14 +160,46 @@ class ModelTurnAssist: self.turn_hold_swept = 0.0 self.turn_hold_handoff_t = 0.0 self.turn_blinker_swept = 0.0 - self.twitch_guard_remaining = 0.0 + self.twitch_guard_armed = False + self.stop_hold_curvature = None + self.output_curvature = None self.curvature = 0.0 self.turn_hold_done = False + def update_stopped(self, CS, new_desired_curvature): + if CS.leftBlinker or CS.rightBlinker or self.output_curvature is None: + self.reset() + return new_desired_curvature + if CS.steeringPressed: + self.output_curvature = self.curvature + return self.output_curvature + def update(self, CS, model_v2, CC, new_desired_curvature): blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) - if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): - new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) + stop_dist = stopping_distance(CS.vEgo, CS.aEgo) + entering = CS.aEgo < -STOP_HOLD_MIN_DECEL and stop_dist < STOP_HOLD_DIST and abs(self.curvature) < STOP_HOLD_MAX_CURVATURE + holding = self.stop_hold_curvature is not None and stop_dist < STOP_HOLD_RELEASE_DIST + if blinker_dir != 0.0: + self.twitch_guard_armed = False + elif (CS.aEgo < -STOP_HOLD_MIN_DECEL and abs(self.curvature) < TWITCH_GUARD_STRAIGHT_LO and + stopping_distance(CS.vEgo, CS.aEgo, TWITCH_GUARD_MAX_SPEED) < STOP_GUARD_DIST): + self.twitch_guard_armed = True + if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0 and (entering or holding): + if CS.steeringPressed: + self.stop_hold_curvature = self.curvature + elif self.stop_hold_curvature is None: + if self.output_curvature is not None: + self.stop_hold_curvature = self.output_curvature + elif self.twitch_guard_armed: + self.stop_hold_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) + else: + self.stop_hold_curvature = new_desired_curvature + new_desired_curvature = self.stop_hold_curvature + self.twitch_guard_armed = True + else: + self.stop_hold_curvature = None + if CC.latActive and self.twitch_guard_armed and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): + new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) if blinker_dir == 0.0: self.turn_blinker_swept = 0.0 else: @@ -250,6 +293,7 @@ class ModelTurnAssist: if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and (not self.turn_hold_done) and (lead_curvature * blinker_dir > abs(self.turn_hold_curvature)): held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) self.turn_hold_curvature = math.copysign(held_mag, lead_curvature) + self.output_curvature = new_desired_curvature return new_desired_curvature diff --git a/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py new file mode 100644 index 0000000000..aceff6d17b --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py @@ -0,0 +1,129 @@ +import unittest +from types import SimpleNamespace + +from openpilot.starpilot.lateral.model_turn_assist import ( + STOP_HOLD_MAX_SPEED, TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED, ModelTurnAssist, update_twitch_guard, +) + +STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[0., 1., 2., 3.], y=[0., 0., 0., 0.]), + meta=SimpleNamespace(laneChangeState=0)) +LONG_STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[float(x) for x in range(0, 80, 4)], y=[0.] * 20), + meta=SimpleNamespace(laneChangeState=0)) + + +def car(speed, accel, blinker=0, pressed=False): + return SimpleNamespace(vEgo=speed, aEgo=accel, rightBlinker=blinker == 1, leftBlinker=blinker == -1, + steeringPressed=pressed, steeringTorque=0.) + + +def step(assist, speed, accel, desired, active=True, model=STRAIGHT, **kwargs): + return assist.update(car(speed, accel, **kwargs), model, SimpleNamespace(latActive=active), desired) + + +class TestStopHold(unittest.TestCase): + def test_holds_command_through_final_metre_of_stop(self): + assist = ModelTurnAssist() + self.assertEqual(step(assist, 2.0, -2.0, .002), .002) + for speed, desired in ((1.5, .004), (1.0, .010), (.6, .014)): + self.assertEqual(step(assist, speed, -2.0, desired), .002) + + def test_releases_when_car_stops_slowing(self): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + self.assertEqual(step(assist, 1.0, -.5, .01), .001) + self.assertEqual(step(assist, 1.0, 0., .01), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_does_not_engage_far_from_stop_or_above_speed(self): + for speed, accel in ((2.0, -.5), (1.0, -.2), (STOP_HOLD_MAX_SPEED, -5.), (1.0, 1.)): + with self.subTest(speed=speed, accel=accel): + assist = ModelTurnAssist() + self.assertEqual(step(assist, speed, accel, .01), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_blinker_or_inactive_never_holds(self): + for direction in (-1, 1): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + self.assertNotEqual(step(assist, 1.0, -2.0, direction * .02, blinker=direction), .001) + self.assertIsNone(assist.stop_hold_curvature) + assist = ModelTurnAssist() + self.assertEqual(step(assist, 1.0, -2.0, .01, active=False), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_driver_steering_moves_hold_to_measured_curvature(self): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + assist.curvature = -.003 + self.assertEqual(step(assist, .8, -2.0, .015, pressed=True), -.003) + assist.curvature = .009 + self.assertEqual(step(assist, .6, -2.0, .015), -.003) + + def test_latches_guarded_command_not_raw_model(self): + assist = ModelTurnAssist() + assist.twitch_guard_armed = True + self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR) + self.assertEqual(step(assist, .6, -2.0, .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR) + + def test_does_not_freeze_a_turn_in_progress(self): + assist = ModelTurnAssist() + assist.curvature = .02 + self.assertEqual(step(assist, 1.0, -2.0, .04), .04) + self.assertIsNone(assist.stop_hold_curvature) + + +class TestTwitchGuardArming(unittest.TestCase): + def test_armed_from_standstill_until_max_speed(self): + self.assertTrue(update_twitch_guard(False, 0., True)) + self.assertTrue(update_twitch_guard(False, .3, False)) + self.assertFalse(update_twitch_guard(False, 1., False)) + for speed in (.5, 2., TWITCH_GUARD_MAX_SPEED - .01): + self.assertTrue(update_twitch_guard(True, speed, False)) + self.assertFalse(update_twitch_guard(True, TWITCH_GUARD_MAX_SPEED, False)) + self.assertFalse(update_twitch_guard(True, float('nan'), False)) + + def test_released_stop_hold_hands_off_to_guard(self): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001, model=LONG_STRAIGHT) + self.assertTrue(assist.twitch_guard_armed) + self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR) + + def test_unarmed_guard_leaves_creep_command(self): + assist = ModelTurnAssist() + self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), .012) + + def test_stop_approach_arms_guard_only_with_straight_wheel(self): + assist = ModelTurnAssist() + self.assertEqual(step(assist, 3.0, -1.5, .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR) + assist = ModelTurnAssist() + assist.curvature = .006 + self.assertEqual(step(assist, 3.0, -1.5, .012, model=LONG_STRAIGHT), .012) + assist = ModelTurnAssist() + self.assertEqual(step(assist, 3.0, -.2, .012, model=LONG_STRAIGHT), .012) + + def test_standstill_keeps_last_command_instead_of_raw_model(self): + assist = ModelTurnAssist() + assist.twitch_guard_armed = True + self.assertEqual(step(assist, 1.0, 0., .012, model=LONG_STRAIGHT), TWITCH_GUARD_FLOOR) + self.assertEqual(assist.update_stopped(car(0., 0.), .012), TWITCH_GUARD_FLOOR) + assist.curvature = -.004 + self.assertEqual(assist.update_stopped(car(0., 0., pressed=True), .012), -.004) + self.assertEqual(assist.update_stopped(car(0., 0.), .012), -.004) + + def test_standstill_passes_raw_model_with_blinker_or_no_history(self): + self.assertEqual(ModelTurnAssist().update_stopped(car(0., 0.), .012), .012) + assist = ModelTurnAssist() + step(assist, 1.0, 0., .003) + self.assertEqual(assist.update_stopped(car(0., 0., blinker=1), .012), .012) + self.assertIsNone(assist.output_curvature) + + def test_blinker_disarms_guard(self): + assist = ModelTurnAssist() + assist.twitch_guard_armed = True + step(assist, 2.0, 0., .012, model=LONG_STRAIGHT, blinker=1) + self.assertFalse(assist.twitch_guard_armed) + self.assertEqual(step(assist, 2.0, 0., .012, model=LONG_STRAIGHT), .012) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 2848423180..9aa4d847de 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -12,8 +12,18 @@ SIGN_CONFIRM_STEP_S = 2 * DT_MDL SIGN_MAX_SPEED_MPS = 17.88 SIGN_MODEL_AGREE_M = 50.0 SIGN_SEED_M = 27.0 +SIGN_ESTIMATE_HOLD_NS = 2_000_000_000 +# Over a long model path a sign is only believed if stopping at its estimate would be a normal stop. +SIGN_LONG_PATH_MAX_DECEL = 2.5 SIGN_APPROACH_DECEL = 1.0 MODEL_APPROACH_DECEL = .65 +# Stopped behind a car at a sign: the line is assumed one car length past that car's rear. +QUEUE_LEAD_MAX_M = 8.0 +QUEUE_LEAD_LENGTH_M = 4.5 +# A car still inside this margin past the line may just be moving up the queue, not through the intersection. +QUEUE_CLEAR_M = 5.0 +QUEUE_SIGN_RECENT_NS = 10_000_000_000 +QUEUE_MAX_SPEED_MPS = 6.0 @dataclass(frozen=True) @@ -58,6 +68,8 @@ class StopFrame: resume_requested: bool = False perception_available: bool = True sign: bool = False + dashboard_sign: bool = False + vision_distance_m: float | None = None @dataclass(frozen=True) @@ -98,6 +110,48 @@ class ForceStop: self.previously_enabled = False self.distance = 0.0 self.distance_cap = 0.0 + self.queue_sign_ns = 0 + self.queue_line = None + self._clear_sign_estimate() + + def _clear_sign_estimate(self): + self.dash_distance = None + self.vision_distance = None + self.vision_samples = 0 + self.sign_evidence_ns = 0 + + def _sign_estimate(self, frame: StopFrame, now: int, travel: float) -> float | None: + if self.sign_evidence_ns and now - self.sign_evidence_ns > SIGN_ESTIMATE_HOLD_NS: + self._clear_sign_estimate() + if self.dash_distance is not None: + self.dash_distance = max(0.0, self.dash_distance - travel) + if self.vision_distance is not None: + self.vision_distance = max(0.0, self.vision_distance - travel) + vision = frame.vision_distance_m + if vision is not None and math.isfinite(vision) and vision >= 0: + self.vision_samples += 1 + self.vision_distance = vision if self.vision_distance is None else ( + self.vision_distance + (vision - self.vision_distance) / self.vision_samples) + if frame.dashboard_sign or frame.sign and self.vision_distance is None: + self.dash_distance = SIGN_SEED_M if self.dash_distance is None else min(self.dash_distance, SIGN_SEED_M) + if frame.sign or vision is not None: + self.sign_evidence_ns = now + estimates = [value for value in (self.dash_distance, self.vision_distance) if value is not None] + return sum(estimates) / len(estimates) if estimates else None + + def _queue_step(self, frame: StopFrame, now: int, travel: float, lead_blocked: bool): + if frame.sign and lead_blocked: + self.queue_sign_ns = now + queued_lead = frame.lead_present and frame.lead_speed_mps < 1.0 and frame.lead_distance_m < QUEUE_LEAD_MAX_M + arm = frame.standstill and self.queue_sign_ns and now - self.queue_sign_ns <= QUEUE_SIGN_RECENT_NS + if queued_lead and (arm or self.queue_line is not None): + self.queue_line = frame.lead_distance_m + QUEUE_LEAD_LENGTH_M + elif self.queue_line is not None: + self.queue_line -= travel + if self.queue_line is not None and (frame.pedal_override or frame.resume_requested or self.queue_line <= 0 or + frame.speed_mps > QUEUE_MAX_SPEED_MPS): + self.queue_line = None + self.queue_sign_ns = 0 def _clear_standstill(self): self.standstill_since_ns = 0 @@ -127,7 +181,10 @@ class ForceStop: self.reset() return StopPlan() available = frame.perception_available - if available and (frame.light or frame.raw_model_stopped) or frame.sign: + sign_estimate = self._sign_estimate(frame, now, max(0.0, speed) * dt) + lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2) + self._queue_step(frame, now, max(0.0, speed) * dt, lead or frame.tracking_lead) + if available and (frame.light or frame.raw_model_stopped) or frame.sign or self.queue_line is not None: if not frame.standstill: self.stop_seen_ns = now if frame.standstill: @@ -135,16 +192,21 @@ class ForceStop: stop_then_turn = bool(self.stop_seen_ns and now - self.stop_seen_ns < 4_000_000_000) turn = speed <= 18 * 0.44704 and frame.blinker and abs(frame.steering_angle_deg) >= 25 and not stop_then_turn curved = available and abs(curvature) >= .003 and not stop_then_turn - lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2) model_active = available and horizon < (108.0 if self.activation_gate and frame.light else 100.0) self.activation_gate = model_active and frame.light permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and not frame.tracking_lead and not lead) model_path = frame.light and model_active and permitted and not curved - sign_path = frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and horizon < SIGN_MODEL_AGREE_M - if sign_path: + plausible = sign_estimate is None or speed * speed <= 2 * SIGN_LONG_PATH_MAX_DECEL * max(sign_estimate, 1.0) + sign_path = (frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and + (horizon < SIGN_MODEL_AGREE_M or self.forcing and plausible)) + line_blocked = (self.queue_line is not None and frame.lead_present and + frame.lead_distance_m < self.queue_line + QUEUE_CLEAR_M) + queue_path = (self.queue_line is not None and not line_blocked and not frame.standstill and available and + now >= self.override_until_ns and not frame.driving_in_curve and not turn) + if sign_path or queue_path: self.sign_confirmed = True - active = model_path or sign_path + active = model_path or sign_path or queue_path if model_path: self.from_light = True self.light_clear_ns = 0 @@ -183,7 +245,7 @@ class ForceStop: if not available and not self.forcing: self.timer = 0 elif active and not frame.standstill: - self.timer = min(2.0, self.timer + (SIGN_CONFIRM_STEP_S if frame.sign else DT_MDL)) + self.timer = min(2.0, self.timer + (SIGN_CONFIRM_STEP_S if frame.sign or queue_path else DT_MDL)) elif turn and not self.forcing and not frame.standstill: self.timer = 0 elif held: @@ -193,7 +255,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) @@ -230,12 +292,12 @@ class ForceStop: elif available: self.distance = min(self.distance, horizon) self.distance_cap = max(0.0, self.distance_cap - speed * dt) - self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1)) - 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 + cap = self.distance_cap + 15 * min(self.distance_cap / 60, 1) + self.distance = min(self.distance, cap) + if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M): + self.distance = min(sign_estimate, cap) + if self.queue_line is not None and self.sign_confirmed: + self.distance = self.queue_line 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 +313,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/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index 720d5345d2..29ccfa326f 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/force_stop_runtime.py @@ -51,6 +51,7 @@ class ForceStopRuntime: self.policy = ForceStop() self.resume = StopResume() self.vision_stop = StopConfirmation() + self.vision_ranged_ns = 0 self.light = StopLightDetector() self.lead = LeadDetector() self.plan = StopPlan() @@ -66,6 +67,7 @@ class ForceStopRuntime: def reset(self): self.vision_stop.reset() + self.vision_ranged_ns = 0 self.resume.reset() self.policy.reset() self.light.reset() @@ -184,14 +186,19 @@ class ForceStopRuntime: return self.plan vision_sign = self.vision_stop.observe( read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id), now_ns=now_ns) + vision_fresh = bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS) + vision_distance = None + if vision_fresh and vision_sign != self.vision_ranged_ns and self.vision_stop.distance_m is not None: + self.vision_ranged_ns = vision_sign + vision_distance = self.vision_stop.distance_m - float(car.vEgo) * (stamp - vision_sign) / 1e9 + dashboard_sign = dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id) frame = StopFrame(stamp, float(car.vEgo), horizon, light_context, horizon is not None and horizon < 50, bool(model.action.shouldStop), bool(car.standstill), True, bool(car.gasPressed), tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead), raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None, bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested, - perception_available=perception_available, - sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id) or - bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS)) + perception_available=perception_available, sign=dashboard_sign or vision_fresh, + dashboard_sign=dashboard_sign, vision_distance_m=vision_distance) self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset) if takeoff_enabled: self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index fe1b9f5a58..2828a85085 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -229,6 +229,138 @@ 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_sign_latches_and_seeds_over_long_path_once_light_committed(self): + for tick in range(15): + plan = self.owner.step(frame(tick, horizon_m=60.), self.tune) + self.assertTrue(plan.forcing) + self.assertGreater(plan.tracked_distance_m, 50.) + plan = self.owner.step(frame(15, horizon_m=60., sign=True), self.tune) + self.assertTrue(self.owner.sign_confirmed) + self.assertLessEqual(plan.tracked_distance_m, 27.) + plan = self.owner.step(frame(16, horizon_m=60., light=False), self.tune) + self.assertLess(plan.tracked_distance_m, 27.) + + def test_long_path_sign_needing_a_hard_stop_is_ignored(self): + for tick in range(15): + plan = self.owner.step(frame(tick, speed_mps=13., horizon_m=90.), self.tune) + self.assertTrue(plan.forcing) + for tick in range(15, 55): + plan = self.owner.step(frame(tick, speed_mps=13., horizon_m=90., sign=True, dashboard_sign=True), self.tune) + self.assertFalse(self.owner.sign_confirmed) + self.assertGreater(plan.tracked_distance_m, 50.) + + def test_long_path_sign_alone_does_not_commit(self): + for tick in range(20): + plan = self.owner.step(frame(tick, horizon_m=60., light=False, sign=True), self.tune) + self.assertFalse(plan.forcing) + self.assertFalse(self.owner.sign_confirmed) + + def test_vision_distance_replaces_assumed_sign_distance(self): + for tick in range(15): + plan = self.owner.step(frame(tick, horizon_m=60.), self.tune) + plan = self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=35.), self.tune) + self.assertTrue(self.owner.sign_confirmed) + self.assertAlmostEqual(plan.tracked_distance_m, 35.) + plan = self.owner.step(frame(16, horizon_m=60., sign=True), self.tune) + self.assertAlmostEqual(plan.tracked_distance_m, 34.5) + plan = self.owner.step(frame(17, horizon_m=60., sign=True, vision_distance_m=31.), self.tune) + self.assertAlmostEqual(plan.tracked_distance_m, 32.5) + + def test_dashboard_and_vision_distances_are_averaged(self): + for tick in range(15): + self.owner.step(frame(tick, horizon_m=60.), self.tune) + plan = self.owner.step(frame(15, horizon_m=60., sign=True, dashboard_sign=True), self.tune) + self.assertAlmostEqual(plan.tracked_distance_m, 27.) + plan = self.owner.step(frame(16, horizon_m=60., sign=True, dashboard_sign=True, vision_distance_m=35.), self.tune) + self.assertAlmostEqual(plan.tracked_distance_m, (26.5 + 35.) / 2) + + def test_sign_distance_expires_without_evidence_and_never_jumps_back(self): + for tick in range(15): + self.owner.step(frame(tick, horizon_m=60.), self.tune) + self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=30.), self.tune) + for tick in range(16, 70): + plan = self.owner.step(frame(tick, speed_mps=4., horizon_m=60., light=True), self.tune) + self.assertLess(plan.tracked_distance_m, 30.) + self.assertIsNone(self.owner.vision_distance) + + def queue_behind_lead(self, sign=True): + for tick in range(10): + plan = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., light=False, sign=sign, lead_present=True, + lead_distance_m=20., lead_speed_mps=0.), self.tune) + self.assertFalse(plan.forcing) + for tick in range(10, 30): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True, + lead_distance_m=5., lead_speed_mps=0.), self.tune) + self.assertFalse(plan.forcing) + + def test_sign_behind_stopped_lead_stops_at_line_after_lead_leaves(self): + self.queue_behind_lead() + self.assertAlmostEqual(self.owner.queue_line, 9.5) + for tick in range(30, 35): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=12., light=False, standstill=True, lead_present=True, + lead_distance_m=7., lead_speed_mps=3.), self.tune) + self.assertFalse(plan.forcing) + for tick in range(35, 45): + plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune) + self.assertTrue(plan.forcing) + self.assertTrue(self.owner.sign_confirmed) + self.assertAlmostEqual(plan.tracked_distance_m, 9.5 - 10 * .05) + for tick in range(45, 100): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=15., light=False, standstill=True), self.tune) + self.assertTrue(plan.manual_hold) + released = self.owner.step(frame(100, speed_mps=0., horizon_m=15., light=False, standstill=True, + pedal_override=True), self.tune) + self.assertFalse(released.manual_hold) + self.assertIsNone(self.owner.queue_line) + + def test_lead_stop_without_sign_is_not_remembered(self): + self.queue_behind_lead(sign=False) + self.assertIsNone(self.owner.queue_line) + for tick in range(30, 50): + plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune) + self.assertFalse(plan.forcing) + + def test_lead_moving_up_the_queue_is_followed_not_stopped_for(self): + self.queue_behind_lead() + for tick in range(30, 60): + plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False, lead_present=True, + lead_distance_m=6., lead_speed_mps=1.5), self.tune) + self.assertFalse(plan.forcing) + plan = self.owner.step(frame(60, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True, + lead_distance_m=4., lead_speed_mps=0.), self.tune) + self.assertFalse(plan.forcing) + self.assertAlmostEqual(self.owner.queue_line, 8.5) + + def test_driver_gas_while_queued_forgets_the_sign(self): + self.queue_behind_lead() + self.owner.step(frame(30, speed_mps=0., horizon_m=5., light=False, standstill=True, pedal_override=True), self.tune) + self.assertIsNone(self.owner.queue_line) + for tick in range(31, 50): + plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune) + self.assertFalse(plan.forcing) + + 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) diff --git a/openpilot/starpilot/speed_limits/vision/stop_observation.py b/openpilot/starpilot/speed_limits/vision/stop_observation.py index 559158906d..8d116492d9 100644 --- a/openpilot/starpilot/speed_limits/vision/stop_observation.py +++ b/openpilot/starpilot/speed_limits/vision/stop_observation.py @@ -2,12 +2,17 @@ from dataclasses import dataclass import math +from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_PAIR_SKEW_NS STOP_MAX_AGE_NS = 1_500_000_000 CONTROL_STOP_MAX_AGE_NS = 750_000_000 RECEIPT_MAX_AGE_NS = 1_000_000_000 STOP_CONFIRM_CONFIDENCE = .7 +STOP_SIGN_HEIGHT_M = .762 +# Fitted on 17 confirmed sign approaches: where the car stops relative to the box range. +STOP_RANGE_OFFSET_M = 2.0 +ROAD_FOCAL_PX = {camera.narrow_road.size: camera.narrow_road.focal_length for camera in DEVICE_CAMERAS.values()} @dataclass(frozen=True) @@ -66,6 +71,16 @@ def read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int, return None +def stop_distance(sample: StopEvidence, box: StopBox) -> float | None: + focal = ROAD_FOCAL_PX.get((sample.width, sample.height)) if sample.stream == 'road' else None + x1, y1, x2, y2 = box.bbox + if not focal or y2 <= y1: + return None + across = ((x1 + x2) / 2 - sample.width / 2) / focal + up = ((y1 + y2) / 2 - sample.height / 2) / focal + return focal * STOP_SIGN_HEIGHT_M / (y2 - y1) / math.sqrt(1 + across ** 2 + up ** 2) + STOP_RANGE_OFFSET_M + + def _matches(a: StopBox, b: StopBox) -> bool: ax, ay, ar, ab = a.bbox bx, by, br, bb = b.bbox @@ -83,6 +98,7 @@ class StopConfirmation: self.previous: StopEvidence | None = None self.confirmed_ns = 0 self.first_seen_ns = 0 + self.distance_m: float | None = None def observe(self, sample: StopEvidence | None, *, now_ns: int) -> int: if sample is None or not 0 <= now_ns - sample.observed_ns < STOP_MAX_AGE_NS: @@ -102,13 +118,15 @@ class StopConfirmation: self.reset() return 0 candidates = tuple(box for box in sample.boxes if box.confidence >= STOP_CONFIRM_CONFIDENCE) - coherent = (previous is not None and 100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and - any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE for new in candidates)) + matched = tuple(new for new in candidates if previous is not None and + 100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and + any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE)) if not candidates: self.first_seen_ns = 0 - elif not coherent: + elif not matched: self.first_seen_ns = sample.observed_ns self.previous = sample - self.confirmed_ns = (sample.observed_ns if candidates and coherent and + self.confirmed_ns = (sample.observed_ns if matched and sample.observed_ns - self.first_seen_ns >= 500_000_000 else 0) + self.distance_m = stop_distance(sample, max(matched, key=lambda box: box.confidence)) if self.confirmed_ns else None return self.confirmed_ns diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py b/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py index 2a3dcf78a8..034708b0c7 100644 --- a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py +++ b/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py @@ -121,6 +121,21 @@ class TestStopObservation(unittest.TestCase): owner.observe(evidence(), now_ns=BASE) self.assertEqual(owner.observe(evidence(BASE + 700_000_000, frame=15, boxes=(box,)), now_ns=BASE + 800_000_000), 0) + def test_confirmed_road_box_carries_forward_distance(self): + box = StopBox(.875, (1214, 400, 1294, 480)) + for stream, expected in (('road', 27.025), ('wideRoad', None)): + owner = StopConfirmation() + owner.observe(evidence(boxes=(box,), stream=stream, width=1928, height=1208), now_ns=BASE) + self.assertIsNone(owner.distance_m) + second = evidence(BASE + 700_000_000, frame=15, boxes=(box,), stream=stream, width=1928, height=1208) + self.assertEqual(owner.observe(second, now_ns=BASE + 800_000_000), second.observed_ns) + if expected is None: + self.assertIsNone(owner.distance_m) + else: + self.assertAlmostEqual(owner.distance_m, expected, places=2) + owner.observe(None, now_ns=BASE + 900_000_000) + self.assertIsNone(owner.distance_m) + def test_brief_warning_burst_cannot_confirm_but_sustained_sign_can(self): owner = StopConfirmation() for i in range(3):