diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 1d0661778a..fc795a36ca 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -46,7 +46,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.rcp is None or self.CP.radarUnavailable: - return super().update(None) + return None vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 78dd594627..39a62b9e43 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -51,7 +51,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.rcp is None: - return super().update(None) + return None vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 99fb89ceb3..0424b95a77 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -44,7 +44,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.rcp is None: - return super().update(None) + return None vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 58f665233b..c05a9ccad6 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +from collections import defaultdict + from cereal import car from opendbc.can.parser import CANParser from selfdrive.car.interfaces import RadarInterfaceBase @@ -28,50 +30,61 @@ class RadarInterface(RadarInterfaceBase): else: self.rcp = _create_nidec_can_parser(CP.carFingerprint) self.trigger_msg = 0x445 - self.updated_messages = set() + self.updated_values = defaultdict(lambda: defaultdict(list)) def update(self, can_strings): # in Bosch radar and we are only steering for now, so sleep 0.05s to keep # radard at 20Hz and return no points if self.radar_off_can: - return super().update(None) + return None - vls = self.rcp.update_strings(can_strings) - self.updated_messages.update(vls) + addresses = self.rcp.update_strings(can_strings) + for addr in addresses: + vals_dict = self.rcp.vl_all[addr] + for sig_name, vals in vals_dict.items(): + self.updated_values[addr][sig_name].extend(vals) - if self.trigger_msg not in self.updated_messages: + if self.trigger_msg not in self.updated_values: return None - rr = self._update(self.updated_messages) - self.updated_messages.clear() - return rr + radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) + self.updated_values.clear() + + return radar_data - def _update(self, updated_messages): + def _radar_msg_from_buffer(self, updated_values, can_valid): ret = car.RadarData.new_message() - for ii in sorted(updated_messages): - cpt = self.rcp.vl[ii] - if ii == 0x400: - # check for radar faults - self.radar_fault = cpt['RADAR_STATE'] != 0x79 - self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69 - elif cpt['LONG_DIST'] < 255: - if ii not in self.pts or cpt['NEW_TRACK']: - self.pts[ii] = car.RadarData.RadarPoint.new_message() - self.pts[ii].trackId = self.track_id - self.track_id += 1 - self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car - self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive - self.pts[ii].vRel = cpt['REL_SPEED'] - self.pts[ii].aRel = float('nan') - self.pts[ii].yvRel = float('nan') - self.pts[ii].measured = True - else: - if ii in self.pts: - del self.pts[ii] + for ii in sorted(updated_values): + msgs = updated_values[ii] + n_vals_per_addr = len(list(msgs.values())[0]) + cpts = [ + {k: v[i] for k, v in msgs.items()} + for i in range(n_vals_per_addr) + ] + + for cpt in cpts: + if ii == 0x400: + # check for radar faults + self.radar_fault = cpt['RADAR_STATE'] != 0x79 + self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69 + elif cpt['LONG_DIST'] < 255: + if ii not in self.pts or cpt['NEW_TRACK']: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car + self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['REL_SPEED'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + else: + if ii in self.pts: + del self.pts[ii] errors = [] - if not self.rcp.can_valid: + if not can_valid: errors.append("canError") if self.radar_fault: errors.append("fault") diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 3b74a03d4b..7301ad18ce 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -30,7 +30,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.radar_off_can or (self.rcp is None): - return super().update(None) + return None vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 99df4cf4b4..0c1a8e3db1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,6 +1,5 @@ import yaml import os -import time from abc import abstractmethod, ABC from typing import Any, Dict, Optional, Tuple, List, Callable @@ -312,14 +311,9 @@ class RadarInterfaceBase(ABC): self.rcp = None self.pts = {} self.delay = 0 - self.radar_ts = CP.radarTimeStep - self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ def update(self, can_strings): - ret = car.RadarData.new_message() - if not self.no_radar_sleep: - time.sleep(self.radar_ts) # radard runs on RI updates - return ret + pass class CarStateBase(ABC): diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index c368c794b0..cf76e213e0 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -36,7 +36,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.rcp is None: - return super().update(None) + return None values = self.rcp.update_strings(can_strings) self.updated_messages.update(values) diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 56473b06a4..378178f5c8 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +from collections import defaultdict + from opendbc.can.parser import CANParser from cereal import car from selfdrive.car.toyota.values import DBC, TSS2_CAR @@ -36,34 +38,47 @@ class RadarInterface(RadarInterfaceBase): self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint) self.trigger_msg = self.RADAR_B_MSGS[-1] - self.updated_messages = set() + self.updated_values = defaultdict(lambda: defaultdict(list)) def update(self, can_strings): if self.rcp is None: - return super().update(None) + return None - vls = self.rcp.update_strings(can_strings) - self.updated_messages.update(vls) + addresses = self.rcp.update_strings(can_strings) + for addr in addresses: + vals_dict = self.rcp.vl_all[addr] + for sig_name, vals in vals_dict.items(): + self.updated_values[addr][sig_name].extend(vals) - if self.trigger_msg not in self.updated_messages: + if self.trigger_msg not in self.updated_values: return None - rr = self._update(self.updated_messages) - self.updated_messages.clear() + radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) + self.updated_values.clear() - return rr + return radar_data - def _update(self, updated_messages): + def _radar_msg_from_buffer(self, updated_values, can_valid): ret = car.RadarData.new_message() errors = [] - if not self.rcp.can_valid: + if not can_valid: errors.append("canError") ret.errors = errors - for ii in sorted(updated_messages): - if ii in self.RADAR_A_MSGS: - cpt = self.rcp.vl[ii] + for ii in sorted(updated_values): + if ii not in self.RADAR_A_MSGS: + continue + + radar_a_msgs = updated_values[ii] + radar_b_msgs = updated_values[ii+16] + n_vals_per_addr = len(list(radar_a_msgs.values())[0]) + cpts = [ + {k: v[i] for k, v in radar_a_msgs.items()} + for i in range(n_vals_per_addr) + ] + + for index, cpt in enumerate(cpts): if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']: self.valid_cnt[ii] = 0 # reset counter if cpt['VALID'] and cpt['LONG_DIST'] < 255: @@ -71,11 +86,15 @@ class RadarInterface(RadarInterfaceBase): else: self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) - score = self.rcp.vl[ii+16]['SCORE'] - # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST'] + n_b_scores = len(radar_b_msgs['SCORE']) + if n_b_scores > 0: + score_index = min(index, n_b_scores - 1) + score = radar_b_msgs['SCORE'][score_index] + else: + score = None # radar point only valid if it's a valid measurement and score is above 50 - if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): + if cpt['VALID'] or (score and score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarData.RadarPoint.new_message() self.pts[ii].trackId = self.track_id diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index cce866a093..3f0a10e85c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,7 +8,7 @@ import capnp from cereal import messaging, log, car from common.numpy_fast import interp from common.params import Params -from common.realtime import Ratekeeper, Priority, config_realtime_process +from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL from system.swaglog import cloudlog from common.kalman.simple_kalman import KF1D @@ -50,7 +50,8 @@ class KalmanParams: class Track: - def __init__(self, v_lead: float, kalman_params: KalmanParams): + def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams): + self.identifier = identifier self.cnt = 0 self.aLeadTau = _LEAD_ACCEL_TAU self.K_A = kalman_params.A @@ -63,9 +64,13 @@ class Track: self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED - self.vLead = v_lead self.measured = measured # measured or estimate + self.update_vlead(v_lead) + + def update_vlead(self, v_lead: float): + self.vLead = v_lead + # computed velocity and accelerations if self.cnt > 0: self.kf.update(self.vLead) @@ -98,11 +103,12 @@ class Track: "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), "aLeadK": float(self.aLeadK), + "aLeadTau": float(self.aLeadTau), "status": True, "fcw": self.is_potential_fcw(model_prob), "modelProb": model_prob, "radar": True, - "aLeadTau": float(self.aLeadTau) + "radarTrackId": self.identifier, } def potential_low_speed_lead(self, v_ego: float): @@ -158,15 +164,17 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), + "status": True, "radar": False, - "status": True + "radarTrackId": -1, } 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]: # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > .5: + lead_msg_empty = any([len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v]]) + if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) else: track = None @@ -174,7 +182,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 not lead_msg_empty and lead_msg.prob > .5: lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: @@ -204,14 +212,14 @@ class RadarD: self.ready = False - def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): + def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]): self.current_time = 1e-9*max(sm.logMonoTime.values()) radar_points = [] radar_errors = [] - if rr is not None: - radar_points = rr.points - radar_errors = rr.errors + if radar_data is not None: + radar_points = radar_data.points + radar_errors = radar_data.errors if sm.updated['carState']: self.v_ego = sm['carState'].vEgo @@ -219,26 +227,32 @@ class RadarD: if sm.updated['modelV2']: self.ready = True - ar_pts = {} - for pt in radar_points: - ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] + if radar_data is not None: + ar_pts = {} + for pt in radar_points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] - # *** remove missing points from meta data *** - for ids in list(self.tracks.keys()): - if ids not in ar_pts: - self.tracks.pop(ids, None) + # *** remove missing points from meta data *** + for ids in list(self.tracks.keys()): + if ids not in ar_pts: + self.tracks.pop(ids, None) - # *** compute the tracks *** - for ids in ar_pts: - rpt = ar_pts[ids] + # *** compute the tracks *** + for ids in ar_pts: + rpt = ar_pts[ids] - # align v_ego by a fixed time to align it with the radar measurement - v_lead = rpt[2] + self.v_ego_hist[0] + # align v_ego by a fixed time to align it with the radar measurement + v_lead = rpt[2] + self.v_ego_hist[0] - # create the track if it doesn't exist or it's a new track - if ids not in self.tracks: - self.tracks[ids] = Track(v_lead, self.kalman_params) - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + # create the track if it doesn't exist or it's a new track + if ids not in self.tracks: + self.tracks[ids] = Track(ids, v_lead, self.kalman_params) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + else: + # *** no radar points, keep existing tracks, update v_lead + for track in self.tracks.values(): + v_lead = track.vRel + self.v_ego_hist[0] + track.update_vlead(v_lead) # *** publish radarState *** self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0 @@ -291,32 +305,33 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface - # *** setup messaging + # setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing + sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"]) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) - RI = RadarInterface(CP) - - rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) + interface = RadarInterface(CP) - while 1: - can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) - rr = RI.update(can_strings) + rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None) + radar = RadarD(DT_MDL, interface.delay) - if rr is None: - continue + while True: + sm.update() - sm.update(0) + if sm.updated['modelV2']: + can_strings = messaging.drain_sock_raw(can_sock) + if len(can_strings) == 0: + radar_data = None + else: + radar_data = interface.update(can_strings) - RD.update(sm, rr) - RD.publish(pm, -rk.remaining*1000.0) + radar.update(sm, radar_data) + radar.publish(pm, -rk.remaining*1000.0) - rk.monitor_time() + rk.monitor_time() def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None): diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 96d625c2cb..1bc6c7034b 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -465,8 +465,8 @@ CONFIGS = [ subs=["radarState", "liveTracks"], ignore=["logMonoTime", "valid", "radarState.cumLagMs"], init_callback=get_car_params_callback, - should_recv_callback=MessageBasedRcvCallback("can"), - main_pub="can", + should_recv_callback=MessageBasedRcvCallback("modelV2"), + unlocked_pubs=["can"], ), ProcessConfig( proc_name="plannerd", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f76b19eddb..3388f0638f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8fd42f02d1ecac695327bfc0afd4be1918ab680a \ No newline at end of file +b3c089c376bb34666110bb6944e8f0501f37780b \ No newline at end of file