diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ea8ab0d4a..3e2c58bde 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -142,8 +142,19 @@ def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool: abs(track.yRel) < G90_RADAR_LOW_SPEED_MAX_Y) +def track_matches_vision(track: Track, lead: capnp._DynamicStructReader, v_ego: float, *, + dist_scale: float, dist_floor: float, vel_limit: float, + y_std_scale: float, y_floor: float) -> bool: + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA + dist_sane = abs(track.dRel - offset_vision_dist) < max(abs(offset_vision_dist) * dist_scale, dist_floor) + vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < vel_limit) or (v_ego + track.vRel > 3) + lat_sane = abs(track.yRel + lead.y[0]) < max(y_floor, y_std_scale * max(float(lead.yStd[0]), 0.2)) + return dist_sane and vel_sane and lat_sane + + def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], - starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False): + starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False, + preferred_track_id: int = -1): if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(starpilot_toggles, "human_lane_changes", False): direction = model_data.meta.laneChangeDirection if direction == LaneChangeDirection.left: @@ -157,9 +168,8 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ if not tracks: return None - offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA - def prob(c): + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) @@ -169,10 +179,21 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ # if no 'sane' match is found return -1 # stationary radar points can be false positives - dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist) * .25, 5.0]) - vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3) - if dist_sane and vel_sane: + if track_matches_vision(track, lead, v_ego, + dist_scale=0.25, dist_floor=5.0, + vel_limit=10.0, y_std_scale=1.0, y_floor=1.0): return track + + # Some vehicles intermittently drop a good radar match on large leads (semis are + # a common offender). If the same track is still present and only missed the + # strict vision gate by a small margin, keep the previous radar match instead of + # oscillating between radar and vision estimates. + preferred_track = tracks.get(preferred_track_id) + if preferred_track is not None and preferred_track.cnt >= 3: + if track_matches_vision(preferred_track, lead, v_ego, + dist_scale=0.40, dist_floor=8.0, + vel_limit=13.0, y_std_scale=2.0, y_floor=1.5): + return preferred_track return None @@ -199,13 +220,15 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, - low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None) -> dict[str, Any]: + low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None, + preferred_track_id: int = -1) -> dict[str, Any]: lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35)) filtered_lead_prob = float(lead_msg.prob if lead_prob is None else lead_prob) # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and filtered_lead_prob > lead_detection_probability: - track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter) + track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter, + preferred_track_id=preferred_track_id) else: track = None @@ -255,6 +278,7 @@ class RadarD: self.kalman_params = KalmanParams(radar_ts) self.g90_radar_filter = g90_radar_filter self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, radar_ts) for _ in range(2)] + self.prev_lead_track_ids = [-1, -1] self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1) @@ -319,10 +343,18 @@ class RadarD: self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True, - g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x) + g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x, + preferred_track_id=self.prev_lead_track_ids[0]) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False, - g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x) + g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x, + preferred_track_id=self.prev_lead_track_ids[1]) + + for i, lead in enumerate((self.radar_state.leadOne, self.radar_state.leadTwo)): + if lead.status and getattr(lead, "radar", False): + self.prev_lead_track_ids[i] = int(getattr(lead, "radarTrackId", -1)) + elif (not lead.status) or (self.prev_lead_track_ids[i] not in self.tracks): + self.prev_lead_track_ids[i] = -1 if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes): self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True) diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py index 31590abea..22ba5de51 100644 --- a/selfdrive/controls/tests/test_leads.py +++ b/selfdrive/controls/tests/test_leads.py @@ -1,13 +1,34 @@ from types import SimpleNamespace import cereal.messaging as messaging +from cereal import log from opendbc.car.toyota.values import CAR as TOYOTA from openpilot.selfdrive.test.process_replay import replay_process_with_name -from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane +from openpilot.selfdrive.controls.radard import ( + g90_low_speed_radar_lead_sane, + g90_radar_lead_lateral_sane, + match_vision_to_track, +) class TestLeads: + @staticmethod + def make_track(d_rel: float, y_rel: float, v_rel: float, cnt: int = 5): + return SimpleNamespace(dRel=d_rel, yRel=y_rel, vRel=v_rel, cnt=cnt) + + @staticmethod + def make_lead(x: float, y: float, v: float, x_std: float = 1.5, y_std: float = 0.3, v_std: float = 1.0): + return SimpleNamespace(x=[x], y=[y], v=[v], xStd=[x_std], yStd=[y_std], vStd=[v_std]) + + @staticmethod + def make_model_data(): + model = log.ModelDataV2.new_message() + model.init('laneLines', 4) + model.meta.laneChangeState = 0 + model.meta.laneChangeDirection = 0 + return model + def test_g90_radar_filters_side_tracks(self): side_track = SimpleNamespace(dRel=13.0, yRel=-10.38, cnt=10) centered_track = SimpleNamespace(dRel=10.8, yRel=-0.21, cnt=5) @@ -22,6 +43,40 @@ class TestLeads: assert g90_low_speed_radar_lead_sane(centered_track, 2.0) assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5) + def test_match_vision_to_track_sticky_holds_preferred_track(self): + v_ego = 21.2 + lead = self.make_lead(x=37.72, y=0.07, v=20.03) + preferred_track = self.make_track(d_rel=46.95, y_rel=0.05, v_rel=-0.58, cnt=8) + tracks = {3647: preferred_track} + + track = match_vision_to_track( + v_ego, + lead, + self.make_model_data(), + tracks, + SimpleNamespace(human_lane_changes=False), + preferred_track_id=3647, + ) + + assert track is preferred_track + + def test_match_vision_to_track_sticky_rejects_bad_preferred_track(self): + v_ego = 21.2 + lead = self.make_lead(x=37.72, y=0.07, v=20.03) + preferred_track = self.make_track(d_rel=58.0, y_rel=2.8, v_rel=-4.5, cnt=8) + tracks = {3647: preferred_track} + + track = match_vision_to_track( + v_ego, + lead, + self.make_model_data(), + tracks, + SimpleNamespace(human_lane_changes=False), + preferred_track_id=3647, + ) + + assert track is None + def test_radar_fault(self): # if there's no radar-related can traffic, radard should either not respond or respond with an error # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check