radard cleanup (#2266)

* some cleanup

* more realistic profiling

* Update selfdrive/controls/radard.py

Co-authored-by: Willem Melching <willem.melching@gmail.com>

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: a77548bff6

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):
+  def update(self, sm, rr, enable_lead):
-    self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
+    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.valid = sm.all_alive_and_valid()
-    dat.radarState.mdMonoTime = sm.logMonoTime['model']
+    radarState = dat.radarState
-    dat.radarState.canMonoTimes = list(rr.canMonoTimes)
+    radarState.mdMonoTime = sm.logMonoTime['model']
-    dat.radarState.radarErrors = list(rr.errors)
+    radarState.canMonoTimes = list(rr.canMonoTimes)
-    dat.radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
+    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)
+      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.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'])
+    sm = messaging.SubMaster(['model', 'controlsState'])
-
-  # *** publish radarState and liveTracks
   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)

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
-      return msg.to_bytes()
 
 
 class PubSocket():
   def send(self, data):
-    pass
+    if not isinstance(data, bytes):
+      data = data.to_bytes()
 
 
 class SubMaster(messaging.SubMaster):

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")