diff --git a/cereal b/cereal index 2689300b4a..774bf79f4d 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 2689300b4aba2a8850a0a8d985f39e2e3954eae9 +Subproject commit 774bf79f4da2b72d01c34a2f2d6ac9388293fbdb diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 0ab8c10b44..89db431d2a 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -52,7 +52,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 e44730ca4f..6c61692b00 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -62,7 +62,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 b86a85f915..26b6aab17c 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 660be4c449..d7fe0f1586 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -38,7 +38,7 @@ class RadarInterface(RadarInterfaceBase): # 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) @@ -54,25 +54,32 @@ class RadarInterface(RadarInterfaceBase): 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] + combined_vals = self.rcp.vl_all[ii] + n_vals_per_addr = len(list(combined_vals.values())[0]) + cpts = [ + {k: v[i] for k, v in combined_vals.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: diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 4ecca542b5..aecfa76233 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -42,7 +42,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 ec9f4a0f06..1eb36ad34e 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 @@ -311,14 +310,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 a09f53e758..431f3fb729 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -58,7 +58,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 64906b34be..bdacdd4ab7 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -45,7 +45,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) @@ -67,33 +67,38 @@ class RadarInterface(RadarInterfaceBase): for ii in sorted(updated_messages): if ii in self.RADAR_A_MSGS: - cpt = self.rcp.vl[ii] - - if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']: - self.valid_cnt[ii] = 0 # reset counter - if cpt['VALID'] and cpt['LONG_DIST'] < 255: - self.valid_cnt[ii] += 1 - 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'] - - # 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 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 = bool(cpt['VALID']) - else: - if ii in self.pts: - del self.pts[ii] + combined_vals = self.rcp.vl_all[ii] + n_vals_per_addr = len(list(combined_vals.values())[0]) + cpts = [ + {k: v[i] for k, v in combined_vals.items()} + for i in range(n_vals_per_addr) + ] + + for cpt in 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: + self.valid_cnt[ii] += 1 + else: + self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) + + score = self.rcp.vl[ii+16]['SCORE'] + + # 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 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 = bool(cpt['VALID']) + else: + if ii in self.pts: + del self.pts[ii] ret.points = list(self.pts.values()) return ret diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bba3ad7cff..8382115e5f 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,14 +164,16 @@ 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 @@ -173,7 +181,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: @@ -203,14 +211,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 @@ -218,26 +226,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 @@ -290,32 +304,30 @@ 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) + 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 ddcc0b2fe8..034fe75b2f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -464,8 +464,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..d259efe04b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8fd42f02d1ecac695327bfc0afd4be1918ab680a \ No newline at end of file +4e55314f3a2502dea71d84e23d4889724c7bd05e \ No newline at end of file