From 6a6e80fa24a6d961f180822a8909a8dbd1f4bc79 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 13 Jun 2026 22:44:00 -0500 Subject: [PATCH] mayhaps --- selfdrive/controls/radard.py | 53 +++++++++++--------------- selfdrive/controls/tests/test_leads.py | 20 +++------- 2 files changed, 28 insertions(+), 45 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 1bfcb6a07a..6a391f4bf6 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,19 +189,13 @@ 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) @@ -215,43 +209,32 @@ def leads_are_duplicate(lead_one: Any, lead_two: Any) -> bool: 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: + if not lead_one_radar or not 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 - ) + 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 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 +275,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,12 +331,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) + g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x) # 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. diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py index 29410a699f..cf1506fbbe 100644 --- a/selfdrive/controls/tests/test_leads.py +++ b/selfdrive/controls/tests/test_leads.py @@ -48,7 +48,7 @@ class TestLeads: 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): + def test_vision_leads_are_not_force_deduped(self): lead_one = { "status": True, "radar": False, @@ -77,27 +77,19 @@ class TestLeads: "modelProb": 0.99, } - assert leads_are_duplicate(lead_one, lead_two) + assert not 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, + radar=True, + radarTrackId=1234, ) lead_two = SimpleNamespace( status=True, - radar=False, - radarTrackId=-1, - dRel=32.2, - vLead=14.1, - yRel=0.05, - modelProb=0.97, + radar=True, + radarTrackId=1234, ) assert leads_are_duplicate(lead_one, lead_two)