HondaDays

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,

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)
 

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()

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:

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)