fixup profiler and add plannerd (#21292)

selfdrive/test/profiling/lib.py

@@ -1,4 +1,4 @@
-from collections import defaultdict
+from collections import defaultdict, deque
 from cereal.services import service_list
 import cereal.messaging as messaging
 import capnp
@@ -44,20 +44,25 @@ class SubMaster(messaging.SubMaster):
     self.rcv_time = {s: 0. for s in services}
     self.rcv_frame = {s: 0 for s in services}
     self.valid = {s: True for s in services}
+    self.recv_dts = {s: deque([0.0] * messaging.AVG_FREQ_HISTORY, maxlen=messaging.AVG_FREQ_HISTORY) for s in services}
     self.logMonoTime = {}
     self.sock = {}
     self.freq = {}
     self.check_average_freq = check_averag_freq
+    self.non_polled_services = []
+    self.ignore_average_freq = []
 
     # TODO: specify multiple triggers for service like plannerd that poll on more than one service
     cur_msgs = []
     self.msgs = []
     msgs = [m for m in msgs if m.which() in services]
+
     for msg in msgs:
       cur_msgs.append(msg)
       if msg.which() == trigger:
         self.msgs.append(cur_msgs)
         cur_msgs = []
+
     self.msgs = list(reversed(self.msgs))
 
     for s in services:

selfdrive/test/profiling/profiler.py

@@ -5,21 +5,25 @@ import cProfile  # pylint: disable=import-error
 import pprofile  # pylint: disable=import-error
 import pyprof2calltree  # pylint: disable=import-error
 
+from cereal import car
 from common.params import Params
 from tools.lib.logreader import LogReader
 from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone
 from selfdrive.test.process_replay.process_replay import CONFIGS
+from selfdrive.car.toyota.values import CAR as TOYOTA
+from selfdrive.car.honda.values import CAR as HONDA
+from selfdrive.car.volkswagen.values import CAR as VW
 
 BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
 
 CARS = {
-  'toyota': ("77611a1fac303767|2020-02-29--13-29-33/3", "TOYOTA COROLLA TSS2 2019"),
-  'honda': ("99c94dc769b5d96e|2019-08-03--14-19-59/2", "HONDA CIVIC 2016 TOURING"),
-  "vw": ("e2a273d7e6eecec2|2021-03-03--16-05-26/4", "AUDI A3"),
+  'toyota': ("0982d79ebb0de295|2021-01-03--20-03-36/6", TOYOTA.RAV4),
+  'honda': ("0982d79ebb0de295|2021-01-08--10-13-10/6", HONDA.CIVIC),
+  "vw": ("ef895f46af5fd73f|2021-05-22--14-06-35/6", VW.AUDI_A3_MK3),
 }
 
 
-def get_inputs(msgs, process):
+def get_inputs(msgs, process, fingerprint):
   for config in CONFIGS:
     if config.proc_name == process:
       sub_socks = list(config.pub_sub.keys())
@@ -29,7 +33,11 @@ def get_inputs(msgs, process):
   # some procs block on CarParams
   for msg in msgs:
     if msg.which() == 'carParams':
-      Params().put("CarParams", msg.as_builder().to_bytes())
+      m = msg.as_builder()
+      m.carParams.carFingerprint = fingerprint
+
+      CP = car.CarParams.from_dict(m.carParams.to_dict())
+      Params().put("CarParams", CP.to_bytes())
       break
 
   sm = SubMaster(msgs, trigger, sub_socks)
@@ -41,7 +49,7 @@ def get_inputs(msgs, process):
   return sm, pm, can_sock
 
 
-def profile(proc, func, car='vw'):
+def profile(proc, func, car='toyota'):
   segment, fingerprint = CARS[car]
   segment = segment.replace('|', '/')
   rlog_url = f"{BASE_URL}{segment}/rlog.bz2"
@@ -59,13 +67,13 @@ def profile(proc, func, car='vw'):
       pass
 
   # Statistical
-  sm, pm, can_sock = get_inputs(msgs, proc)
+  sm, pm, can_sock = get_inputs(msgs, proc, fingerprint)
   with pprofile.StatisticalProfile()(period=0.00001) as pr:
     run(sm, pm, can_sock)
   pr.dump_stats(f'cachegrind.out.{proc}_statistical')
 
   # Deterministic
-  sm, pm, can_sock = get_inputs(msgs, proc)
+  sm, pm, can_sock = get_inputs(msgs, proc, fingerprint)
   with cProfile.Profile() as pr:
     run(sm, pm, can_sock)
   pyprof2calltree.convert(pr.getstats(), f'cachegrind.out.{proc}_deterministic')
@@ -75,11 +83,13 @@ if __name__ == '__main__':
   from selfdrive.controls.controlsd import main as controlsd_thread
   from selfdrive.controls.radard import radard_thread
   from selfdrive.locationd.paramsd import main as paramsd_thread
+  from selfdrive.controls.plannerd import main as plannerd_thread
 
   procs = {
     'radard': radard_thread,
     'controlsd': controlsd_thread,
     'paramsd': paramsd_thread,
+    'plannerd': plannerd_thread,
   }
 
   proc = sys.argv[1]