diff --git a/cereal b/cereal index 774bf79f4d..2689300b4a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 774bf79f4da2b72d01c34a2f2d6ac9388293fbdb +Subproject commit 2689300b4aba2a8850a0a8d985f39e2e3954eae9 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 89db431d2a..0ab8c10b44 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 None + return super().update(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 6c61692b00..e44730ca4f 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 None + return super().update(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 26b6aab17c..b86a85f915 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 None + return super().update(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 d7fe0f1586..660be4c449 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 None + return super().update(None) vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) @@ -54,32 +54,25 @@ class RadarInterface(RadarInterfaceBase): ret = car.RadarData.new_message() for ii in sorted(updated_messages): - 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] + 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] errors = [] if not self.rcp.can_valid: diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index aecfa76233..4ecca542b5 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 None + return super().update(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 1eb36ad34e..ec9f4a0f06 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,5 +1,6 @@ import yaml import os +import time from abc import abstractmethod, ABC from typing import Any, Dict, Optional, Tuple, List, Callable @@ -310,9 +311,14 @@ 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): - pass + ret = car.RadarData.new_message() + if not self.no_radar_sleep: + time.sleep(self.radar_ts) # radard runs on RI updates + return ret class CarStateBase(ABC): diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index 431f3fb729..a09f53e758 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 None + return super().update(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 bdacdd4ab7..64906b34be 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 None + return super().update(None) vls = self.rcp.update_strings(can_strings) self.updated_messages.update(vls) @@ -67,38 +67,33 @@ class RadarInterface(RadarInterfaceBase): for ii in sorted(updated_messages): if ii in self.RADAR_A_MSGS: - 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] + 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] ret.points = list(self.pts.values()) return ret diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8382115e5f..bba3ad7cff 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, DT_MDL +from common.realtime import Ratekeeper, Priority, config_realtime_process from system.swaglog import cloudlog from common.kalman.simple_kalman import KF1D @@ -50,8 +50,7 @@ class KalmanParams: class Track: - def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams): - self.identifier = identifier + def __init__(self, v_lead: float, kalman_params: KalmanParams): self.cnt = 0 self.aLeadTau = _LEAD_ACCEL_TAU self.K_A = kalman_params.A @@ -64,12 +63,8 @@ class Track: self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED - self.measured = measured # measured or estimate - - self.update_vlead(v_lead) - - def update_vlead(self, v_lead: float): self.vLead = v_lead + self.measured = measured # measured or estimate # computed velocity and accelerations if self.cnt > 0: @@ -103,12 +98,11 @@ 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, - "radarTrackId": self.identifier, + "aLeadTau": float(self.aLeadTau) } def potential_low_speed_lead(self, v_ego: float): @@ -164,16 +158,14 @@ 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, - "radarTrackId": -1, + "status": True } 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 - 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: + if len(tracks) > 0 and ready and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) else: track = None @@ -181,7 +173,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 not lead_msg_empty and lead_msg.prob > .5: + elif (track is None) and ready and (lead_msg.prob > .5): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: @@ -211,14 +203,14 @@ class RadarD: self.ready = False - def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]): + def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): self.current_time = 1e-9*max(sm.logMonoTime.values()) radar_points = [] radar_errors = [] - if radar_data is not None: - radar_points = radar_data.points - radar_errors = radar_data.errors + if rr is not None: + radar_points = rr.points + radar_errors = rr.errors if sm.updated['carState']: self.v_ego = sm['carState'].vEgo @@ -226,32 +218,26 @@ class RadarD: if sm.updated['modelV2']: self.ready = True - 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] + 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(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) + # 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]) # *** publish radarState *** self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0 @@ -304,30 +290,32 @@ 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'], poll=["modelV2"]) + sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) - interface = RadarInterface(CP) + RI = RadarInterface(CP) - rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None) - radar = RadarD(DT_MDL, interface.delay) + rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) + RD = RadarD(CP.radarTimeStep, RI.delay) - while True: - sm.update() + while 1: + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) - if sm.updated['modelV2']: - can_strings = messaging.drain_sock_raw(can_sock) - radar_data = interface.update(can_strings) + if rr is None: + continue + + sm.update(0) - radar.update(sm, radar_data) - radar.publish(pm, -rk.remaining*1000.0) + RD.update(sm, rr) + RD.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 034fe75b2f..ddcc0b2fe8 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("modelV2"), - unlocked_pubs=["can"], + should_recv_callback=MessageBasedRcvCallback("can"), + main_pub="can", ), ProcessConfig( proc_name="plannerd", diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d259efe04b..f76b19eddb 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4e55314f3a2502dea71d84e23d4889724c7bd05e \ No newline at end of file +8fd42f02d1ecac695327bfc0afd4be1918ab680a \ No newline at end of file