diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 21146ede47..a665240f25 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -9,6 +9,9 @@ VISION_LEAD_TRACK_MIN_DISTANCE = 25.0 VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75 VISION_LEAD_TRACK_CLOSING_GAIN = 0.20 VISION_LEAD_TRACK_CLOSING_CAP = 2.50 +VISION_LEAD_TRACK_EXIT_TIME_GAP = 2.30 +VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET = 1.6 +VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB = 0.70 TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40 TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70 TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75 @@ -50,6 +53,21 @@ def should_track_lead(lead_status: bool, lead_distance: float, model_length: flo return float(lead_distance) < min(model_limit, vision_limit) +def should_hold_tracked_vision_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float, + v_ego: float, *, model_prob: float, + y_rel: float, path_y: float = 0.0, radar: bool = False) -> bool: + if not lead_status or radar or float(model_prob) < VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB: + return False + if abs(float(y_rel) + float(path_y)) > VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET: + return False + + tracking_buffer = max(float(stop_distance), 4.0) + model_limit = float(model_length) + tracking_buffer + vision_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, + float(v_ego) * VISION_LEAD_TRACK_EXIT_TIME_GAP + tracking_buffer) + return float(lead_distance) < min(model_limit, vision_exit_limit) + + def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *, radar: bool = False, lead_brake: float = 0.0, lead_prob: float = 0.0, diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 2135613fde..7ef737060d 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -1,6 +1,7 @@ from openpilot.selfdrive.controls.lib.lead_behavior import ( get_tracked_lead_catchup_bias, is_radarless_matched_follow_window, + should_hold_tracked_vision_lead, should_track_lead, should_disable_far_lead_throttle, ) @@ -104,6 +105,59 @@ def test_should_track_lead_accepts_fast_closing_vision_lead_early(): assert should_track_lead(True, 90.0, 140.0, 6.0, 20.0, v_lead=0.0, radar=False) +def test_should_hold_tracked_vision_lead_keeps_honda_bookmark_case(): + assert should_hold_tracked_vision_lead( + True, 44.5, 174.0, 6.0, 16.8, + model_prob=0.99, y_rel=-0.69, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_does_not_expand_initial_tracking_gate(): + assert not should_track_lead(True, 44.5, 174.0, 6.0, 16.8, v_lead=16.7, radar=False) + + +def test_should_hold_tracked_vision_lead_releases_offcenter_lead(): + assert not should_hold_tracked_vision_lead( + True, 44.5, 174.0, 6.0, 16.8, + model_prob=0.99, y_rel=-1.7, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_uses_path_relative_offset_on_curve(): + assert should_hold_tracked_vision_lead( + True, 27.8, 174.0, 6.0, 16.0, + model_prob=1.0, y_rel=-2.11, path_y=1.32, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_releases_low_confidence_lead(): + assert not should_hold_tracked_vision_lead( + True, 44.5, 174.0, 6.0, 16.8, + model_prob=0.69, y_rel=0.0, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_does_not_change_radar_tracking(): + assert not should_hold_tracked_vision_lead( + True, 44.5, 174.0, 6.0, 16.8, + model_prob=0.99, y_rel=0.0, radar=True, + ) + + +def test_should_hold_tracked_vision_lead_keeps_braking_lead(): + assert should_hold_tracked_vision_lead( + True, 44.5, 174.0, 6.0, 16.8, + model_prob=0.99, y_rel=0.0, radar=False, + ) + + +def test_should_hold_tracked_vision_lead_releases_beyond_exit_gap(): + assert not should_hold_tracked_vision_lead( + True, 57.0, 174.0, 6.0, 16.8, + model_prob=0.99, y_rel=0.0, radar=False, + ) + + def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow(): assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95) diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index cfa995c5b0..080cf4ab2a 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -158,3 +158,46 @@ def test_prioritize_smooth_following_skips_radarless_follow_hold(monkeypatch): planner.shutdown() if 'planner_smooth' in locals(): planner_smooth.shutdown() + + +def test_tracked_vision_lead_uses_exit_hysteresis_at_mid_speed(): + planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) + + try: + planner.model_length = 174.0 + planner.tracking_lead = True + planner.tracking_lead_filter.x = 1.0 + planner.lead_one = SimpleNamespace( + status=True, + dRel=44.5, + vLead=16.7, + yRel=-0.69, + aLeadK=0.0, + modelProb=0.99, + radar=False, + ) + + assert planner.update_lead_status(16.8, stop_distance=6.0, prioritize_smooth_following=False) + assert planner.update_lead_status(16.8, stop_distance=6.0, prioritize_smooth_following=True) + finally: + planner.shutdown() + + +def test_untracked_vision_lead_still_uses_strict_entry_gate(): + planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) + + try: + planner.model_length = 174.0 + planner.lead_one = SimpleNamespace( + status=True, + dRel=44.5, + vLead=16.7, + yRel=-0.69, + aLeadK=0.0, + modelProb=0.99, + radar=False, + ) + + assert not planner.update_lead_status(16.8, stop_distance=6.0, prioritize_smooth_following=False) + finally: + planner.shutdown() diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index beae168938..efa118788c 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -13,7 +13,7 @@ from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper, DT_MDL from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.lead_behavior import should_track_lead +from openpilot.selfdrive.controls.lib.lead_behavior import should_hold_tracked_vision_lead, should_track_lead from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE @@ -200,6 +200,18 @@ class Plant: v_lead=float(v_lead), radar=bool(self.only_radar), ) + continuity_candidate = self.tracking_lead_filter.x >= THRESHOLD * 0.6 + if not tracking_candidate and continuity_candidate: + tracking_candidate = should_hold_tracked_vision_lead( + status, + float(d_rel), + float(position.x[-1]) if len(position.x) else 0.0, + STOP_DISTANCE, + float(self.speed), + model_prob=float(prob_lead), + y_rel=float(lead.yRel), + radar=bool(self.only_radar), + ) self.tracking_lead_filter.update(tracking_candidate) tracking_lead = self.tracking_lead_filter.x >= THRESHOLD else: diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 8d196ca435..8f4ce0096e 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -4,6 +4,7 @@ import math import time import cereal.messaging as messaging +import numpy as np from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter @@ -11,7 +12,11 @@ from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET -from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window, should_track_lead +from openpilot.selfdrive.controls.lib.lead_behavior import ( + is_radarless_matched_follow_window, + should_hold_tracked_vision_lead, + should_track_lead, +) from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature @@ -77,6 +82,7 @@ class StarPilotPlanner: self._lane_width_counter = 0 self.lateral_acceleration = 0 self.model_length = 0 + self.lead_path_y = 0 self.road_curvature = 0 self.time_to_curve = 0 self.v_cruise = 0 @@ -183,6 +189,12 @@ class StarPilotPlanner: self.CS_prev_right_blinker = CS.rightBlinker self.model_length = sm["modelV2"].position.x[-1] + model_position = sm["modelV2"].position + model_path_y = getattr(model_position, "y", []) + if len(model_path_y) == len(model_position.x): + self.lead_path_y = float(np.interp(self.lead_one.dRel, model_position.x, model_path_y)) + else: + self.lead_path_y = 0.0 self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop @@ -233,6 +245,19 @@ class StarPilotPlanner: v_lead=self.lead_one.vLead, radar=bool(getattr(self.lead_one, "radar", False)), ) + continuity_candidate = self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6 + if not following_lead and continuity_candidate: + following_lead = should_hold_tracked_vision_lead( + self.lead_one.status, + self.lead_one.dRel, + self.model_length, + stop_distance, + v_ego, + model_prob=float(getattr(self.lead_one, "modelProb", 0.0)), + y_rel=float(getattr(self.lead_one, "yRel", 0.0)), + path_y=self.lead_path_y, + radar=bool(getattr(self.lead_one, "radar", False)), + ) now_t = time.monotonic() lead_radar = bool(getattr(self.lead_one, "radar", False)) t_follow = max(float(getattr(self.starpilot_following, "t_follow", 0.0)), 1.45)