mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-05 16:26:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

oopsie

Browse Source
This commit is contained in:
firestar5683
2026-06-13 21:48:35 -05:00
parent 4a107868cf
commit 7bd73401cd
2 changed files with 39 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+17 -11
View File
@@ -202,30 +202,36 @@ VISION_DUPLICATE_LEAD_MAX_YREL_DIFF = 0.4
VISION_DUPLICATE_LEAD_MIN_MODEL_PROB = 0.7
def leads_are_duplicate(lead_one: dict[str, Any], lead_two: dict[str, Any]) -> bool:
if not lead_one.get("status", False) or not lead_two.get("status", False):
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(lead_one.get("radar", False))
lead_two_radar = bool(lead_two.get("radar", 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(lead_one.get("radarTrackId", -1))
lead_two_track_id = int(lead_two.get("radarTrackId", -1))
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(lead_one.get("modelProb", 0.0))
lead_two_prob = float(lead_two.get("modelProb", 0.0))
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(lead_one.get("dRel", 0.0)) - float(lead_two.get("dRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_DREL_DIFF and
abs(float(lead_one.get("vLead", 0.0)) - float(lead_two.get("vLead", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_VLEAD_DIFF and
abs(float(lead_one.get("yRel", 0.0)) - float(lead_two.get("yRel", 0.0))) <= VISION_DUPLICATE_LEAD_MAX_YREL_DIFF
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
)
selfdrive/controls/tests/test_leads.py
+22
View File
@@ -80,6 +80,28 @@ class TestLeads:
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