brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Tweak lead detection threshold

Browse Source 
Added toggle to tweak the lead detection threshold to either detect leads sooner, or increase model confidence.
This commit is contained in:
FrogAi
2024-03-15 20:53:30 -07:00
parent bed144260f
commit 40a36dfcde
4 changed files with 23 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
selfdrive/controls/radard.py
View File

@@ -167,9 +167,9 @@ 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, low_speed_override: bool = True) -> Dict[str, Any]:
model_v_ego: float, lead_detection_threshold: float = .5, low_speed_override: bool = True) -> Dict[str, Any]:
# Determine leads, this is where the essential logic happens
if len(tracks) > 0 and ready and lead_msg.prob > .5:
if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_threshold:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
@@ -177,7 +177,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
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 > .5):
elif (track is None) and ready and (lead_msg.prob > lead_detection_threshold):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override:
@@ -208,6 +208,12 @@ class RadarD:
self.ready = False
# FrogPilot variables
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.update_frogpilot_params()
def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
self.ready = sm.seen['modelV2']
self.current_time = 1e-9*max(sm.logMonoTime.values())
@@ -257,8 +263,12 @@ class RadarD:
model_v_ego = self.v_ego
leads_v3 = sm['modelV2'].leadsV3
if len(leads_v3) > 1:
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_detection_threshold, low_speed_override=True)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_detection_threshold, low_speed_override=False)
# Update FrogPilot parameters
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
self.update_frogpilot_params()
def publish(self, pm: messaging.PubMaster, lag_ms: float):
assert self.radar_state is not None
@@ -281,6 +291,9 @@ class RadarD:
}
pm.send('liveTracks', tracks_msg)
def update_frogpilot_params(self):
longitudinal_tune = self.params.get_bool("LongitudinalTune")
self.lead_detection_threshold = self.params.get_int("LeadDetectionThreshold") / 100 if longitudinal_tune else .5
# fuses camera and radar data for best lead detection
def main():