diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 917f5d9741..6cb5f36a0a 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -91,19 +91,16 @@ class RadarD(): self.tracks = defaultdict(dict) self.kalman_params = KalmanParams(radar_ts) - self.active = 0 - # v_ego self.v_ego = 0. self.v_ego_hist = deque([0], maxlen=delay+1) self.ready = False - def update(self, frame, sm, rr, enable_lead): - self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) + def update(self, sm, rr, enable_lead): + self.current_time = 1e-9*max(sm.logMonoTime.values()) if sm.updated['controlsState']: - self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['model']: @@ -160,15 +157,16 @@ class RadarD(): # *** publish radarState *** dat = messaging.new_message('radarState') - dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) - dat.radarState.mdMonoTime = sm.logMonoTime['model'] - dat.radarState.canMonoTimes = list(rr.canMonoTimes) - dat.radarState.radarErrors = list(rr.errors) - dat.radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] + dat.valid = sm.all_alive_and_valid() + radarState = dat.radarState + radarState.mdMonoTime = sm.logMonoTime['model'] + radarState.canMonoTimes = list(rr.canMonoTimes) + radarState.radarErrors = list(rr.errors) + radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] if enable_lead: - dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) - dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) return dat @@ -185,13 +183,11 @@ def radard_thread(sm=None, pm=None, can_sock=None): cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface + # *** setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') - if sm is None: - sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) - - # *** publish radarState and liveTracks + sm = messaging.SubMaster(['model', 'controlsState']) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) @@ -212,7 +208,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): sm.update(0) - dat = RD.update(rk.frame, sm, rr, enable_lead) + dat = RD.update(sm, rr, enable_lead) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) diff --git a/selfdrive/test/profiling/lib.py b/selfdrive/test/profiling/lib.py index 017b9a2588..1694e12aca 100644 --- a/selfdrive/test/profiling/lib.py +++ b/selfdrive/test/profiling/lib.py @@ -11,7 +11,7 @@ class SubSocket(): def __init__(self, msgs, trigger): self.i = 0 self.trigger = trigger - self.msgs = [m for m in msgs if m.which() == trigger] + self.msgs = [m.as_builder().to_bytes() for m in msgs if m.which() == trigger] self.max_i = len(self.msgs) - 1 def receive(self, non_blocking=False): @@ -23,16 +23,14 @@ class SubSocket(): while True: msg = self.msgs[self.i] - msg = msg.as_builder() - self.i += 1 - - return msg.to_bytes() + return msg class PubSocket(): def send(self, data): - pass + if not isinstance(data, bytes): + data = data.to_bytes() class SubMaster(messaging.SubMaster): diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py index 1364194e52..c4bfe15957 100755 --- a/selfdrive/test/profiling/profiler.py +++ b/selfdrive/test/profiling/profiler.py @@ -32,7 +32,7 @@ def get_inputs(msgs, process): def profile(proc, func, car='toyota'): - segment, fingerprint = CARS['toyota'] + segment, fingerprint = CARS[car] segment = segment.replace('|', '/') rlog_url = f"{BASE_URL}{segment}/rlog.bz2" msgs = list(LogReader(rlog_url)) @@ -66,7 +66,7 @@ if __name__ == '__main__': 'radard': radard_thread, 'controlsd': controlsd_thread, } - + proc = sys.argv[1] if proc not in procs: print(f"{proc} not available")