From 1a2818d7758eaa145af80afd7390206db520c86f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 18 Aug 2023 22:56:43 -0700 Subject: [PATCH] Revert "radard: tie radard frequency to modelV2 vol. 2 (#29240)" (#29474) * Revert "static analysis fixes" This reverts commit 2271d0adf9a02aee1359717c47cb6280893a6b9b. * Revert "radard: tie radard frequency to modelV2 vol. 2 (#29240)" This reverts commit 6757e90415766e84ad1b6c921a2edb29ceb3bbbe. * update refs old-commit-hash: 1a08c2986e5600dd4e84bfeaed26c74e679085de --- selfdrive/car/chrysler/radar_interface.py | 2 +- selfdrive/car/ford/radar_interface.py | 2 +- selfdrive/car/gm/radar_interface.py | 2 +- selfdrive/car/honda/radar_interface.py | 73 ++++++-------- selfdrive/car/hyundai/radar_interface.py | 2 +- selfdrive/car/interfaces.py | 8 +- selfdrive/car/tesla/radar_interface.py | 2 +- selfdrive/car/toyota/radar_interface.py | 51 +++------- selfdrive/controls/radard.py | 99 ++++++++----------- .../test/process_replay/process_replay.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- 11 files changed, 103 insertions(+), 144 deletions(-) diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index fc795a36ca..1d0661778a 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 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 39a62b9e43..78dd594627 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 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 0424b95a77..99fb89ceb3 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 43fe422026..58f665233b 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -from collections import defaultdict - from cereal import car from opendbc.can.parser import CANParser from selfdrive.car.interfaces import RadarInterfaceBase @@ -30,61 +28,50 @@ class RadarInterface(RadarInterfaceBase): else: self.rcp = _create_nidec_can_parser(CP.carFingerprint) self.trigger_msg = 0x445 - self.updated_values = defaultdict(lambda: defaultdict(list)) + self.updated_messages = set() 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 None + return super().update(None) - 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) + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) - if self.trigger_msg not in self.updated_values: + if self.trigger_msg not in self.updated_messages: return None - radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) - self.updated_values.clear() + rr = self._update(self.updated_messages) + self.updated_messages.clear() + return rr - return radar_data - - def _radar_msg_from_buffer(self, updated_values, can_valid): + def _update(self, updated_messages): ret = car.RadarData.new_message() - 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] + 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] errors = [] - if not can_valid: + if not self.rcp.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 7301ad18ce..3b74a03d4b 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 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 a9966c2bb7..f30683d9a3 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 @@ -309,9 +310,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 cf76e213e0..c368c794b0 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 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 378178f5c8..56473b06a4 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,6 +1,4 @@ #!/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 @@ -38,47 +36,34 @@ 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_values = defaultdict(lambda: defaultdict(list)) + self.updated_messages = set() def update(self, can_strings): if self.rcp is None: - return None + return super().update(None) - 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) + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) - if self.trigger_msg not in self.updated_values: + if self.trigger_msg not in self.updated_messages: return None - radar_data = self._radar_msg_from_buffer(self.updated_values, self.rcp.can_valid) - self.updated_values.clear() + rr = self._update(self.updated_messages) + self.updated_messages.clear() - return radar_data + return rr - def _radar_msg_from_buffer(self, updated_values, can_valid): + def _update(self, updated_messages): ret = car.RadarData.new_message() errors = [] - if not can_valid: + if not self.rcp.can_valid: errors.append("canError") ret.errors = errors - 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] + for ii in sorted(updated_messages): + if ii in self.RADAR_A_MSGS: + cpt = self.rcp.vl[ii] - 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: @@ -86,15 +71,11 @@ class RadarInterface(RadarInterfaceBase): else: self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0) - 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 + 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 and score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): + 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 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 303bbaaafe..cce866a093 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,17 +158,15 @@ 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 @@ -182,7 +174,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: @@ -212,14 +204,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 @@ -227,32 +219,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 @@ -305,33 +291,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) - if len(can_strings) == 0: - radar_data = None - else: - 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 08e497e72d..623478c555 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("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 ce6c513c10..8aec881aa5 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ac3f87bad519df408a295d2e007e0de58b11690c \ No newline at end of file +72e3d7b660ee92f5adcc249112cf04c703f4bf9e \ No newline at end of file