diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 1bfcb6a07..ea8ab0d4a 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -176,7 +176,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ return None -def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): +def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, model_prob: float): prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK get_RadarState_from_vision.prev_aLeadK = blended_aLeadK @@ -189,69 +189,31 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, - "modelProb": float(lead_msg.prob), + "modelProb": float(model_prob), "status": True, "radar": False, "radarTrackId": -1, } -VISION_DUPLICATE_LEAD_MAX_DREL_DIFF = 0.75 -VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF = 0.5 -VISION_DUPLICATE_LEAD_MAX_YREL_DIFF = 0.4 -VISION_DUPLICATE_LEAD_MIN_MODEL_PROB = 0.7 - - -def get_lead_field(lead: Any, field: str, default: Any) -> Any: - if isinstance(lead, dict): - return lead.get(field, default) - return getattr(lead, field, default) - - -def leads_are_duplicate(lead_one: Any, lead_two: Any) -> bool: - if not get_lead_field(lead_one, "status", False) or not get_lead_field(lead_two, "status", False): - return False - - lead_one_radar = bool(get_lead_field(lead_one, "radar", False)) - lead_two_radar = bool(get_lead_field(lead_two, "radar", False)) - - if lead_one_radar and lead_two_radar: - lead_one_track_id = int(get_lead_field(lead_one, "radarTrackId", -1)) - lead_two_track_id = int(get_lead_field(lead_two, "radarTrackId", -1)) - return lead_one_track_id != -1 and lead_one_track_id == lead_two_track_id - - if lead_one_radar or lead_two_radar: - return False - - lead_one_prob = float(get_lead_field(lead_one, "modelProb", 0.0)) - lead_two_prob = float(get_lead_field(lead_two, "modelProb", 0.0)) - if min(lead_one_prob, lead_two_prob) < VISION_DUPLICATE_LEAD_MIN_MODEL_PROB: - return False - - return ( - abs(float(get_lead_field(lead_one, "dRel", 0.0)) - float(get_lead_field(lead_two, "dRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_DREL_DIFF and - abs(float(get_lead_field(lead_one, "vLead", 0.0)) - float(get_lead_field(lead_two, "vLead", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF and - abs(float(get_lead_field(lead_one, "yRel", 0.0)) - float(get_lead_field(lead_two, "yRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_YREL_DIFF - ) - - 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) -> dict[str, Any]: + low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None) -> 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 lead_msg.prob > lead_detection_probability: + 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) else: track = None lead_dict = {'status': False} if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) - elif (track is None) and ready and (lead_msg.prob > lead_detection_probability): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + lead_dict = track.get_RadarState(filtered_lead_prob) + elif (track is None) and ready and (filtered_lead_prob > lead_detection_probability): + lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, filtered_lead_prob) if low_speed_override: if g90_radar_filter: @@ -292,6 +254,7 @@ class RadarD: self.tracks: dict[int, Track] = {} 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.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1) @@ -347,17 +310,19 @@ class RadarD: leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: + for i in range(2): + lead_prob = float(leads_v3[i].prob) + if lead_prob > self.lead_prob_filters[i].x: + self.lead_prob_filters[i].x = lead_prob + else: + self.lead_prob_filters[i].update(lead_prob) + 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) + g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x) 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) - # The model exposes two lead slots, but both can occasionally fuse to the - # same radar object. Publishing that as two separate leads makes MPC churn - # between lead0/lead1 even though the scene only has one physical target. - if leads_are_duplicate(self.radar_state.leadOne, self.radar_state.leadTwo): - self.radar_state.leadTwo = {'status': False} + g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x) 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 29410a699..31590abea 100644 --- a/selfdrive/controls/tests/test_leads.py +++ b/selfdrive/controls/tests/test_leads.py @@ -4,7 +4,7 @@ import cereal.messaging as messaging 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, leads_are_duplicate +from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane class TestLeads: @@ -22,86 +22,6 @@ 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_duplicate_radar_leads_share_track(self): - lead_one = { - "status": True, - "radar": True, - "radarTrackId": 20293, - } - lead_two = { - "status": True, - "radar": True, - "radarTrackId": 20293, - } - different_track = { - "status": True, - "radar": True, - "radarTrackId": 20326, - } - vision_only = { - "status": True, - "radar": False, - "radarTrackId": -1, - } - - assert leads_are_duplicate(lead_one, lead_two) - assert not leads_are_duplicate(lead_one, different_track) - assert not leads_are_duplicate(lead_one, vision_only) - - def test_duplicate_vision_leads_are_deduped(self): - lead_one = { - "status": True, - "radar": False, - "radarTrackId": -1, - "dRel": 48.3, - "vLead": 18.9, - "yRel": 0.14, - "modelProb": 0.99, - } - lead_two = { - "status": True, - "radar": False, - "radarTrackId": -1, - "dRel": 48.4, - "vLead": 19.0, - "yRel": 0.14, - "modelProb": 1.00, - } - distinct_lead = { - "status": True, - "radar": False, - "radarTrackId": -1, - "dRel": 48.3, - "vLead": 18.9, - "yRel": 1.10, - "modelProb": 0.99, - } - - assert leads_are_duplicate(lead_one, lead_two) - assert not leads_are_duplicate(lead_one, distinct_lead) - - def test_duplicate_lead_helper_supports_attribute_objects(self): - lead_one = SimpleNamespace( - status=True, - radar=False, - radarTrackId=-1, - dRel=32.1, - vLead=14.2, - yRel=0.03, - modelProb=0.98, - ) - lead_two = SimpleNamespace( - status=True, - radar=False, - radarTrackId=-1, - dRel=32.2, - vLead=14.1, - yRel=0.05, - modelProb=0.97, - ) - - assert leads_are_duplicate(lead_one, lead_two) - 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