mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-27 18:03:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

mayhaps

Browse Source
This commit is contained in:
firestar5683
2026-06-13 22:44:00 -05:00
parent 7bd73401cd
commit 6a6e80fa24
2 changed files with 28 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+22 -31
View File
@@ -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.
selfdrive/controls/tests/test_leads.py
+6 -14
View File
@@ -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)