From 66455b075d918d9f04fae6921d0a692ee3f8afd0 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Tue, 2 Jun 2020 17:32:55 -0700
Subject: [PATCH] locationd profiling (#1625)

---
 selfdrive/test/profiling/lib.py       |  2 ++
 selfdrive/test/profiling/locationd.py | 49 +++++++++++++++++++++++++++
 2 files changed, 51 insertions(+)
 create mode 100755 selfdrive/test/profiling/locationd.py

diff --git a/selfdrive/test/profiling/lib.py b/selfdrive/test/profiling/lib.py
index 0495be80c3..997b48f673 100644
--- a/selfdrive/test/profiling/lib.py
+++ b/selfdrive/test/profiling/lib.py
@@ -82,6 +82,8 @@ class SubMaster(messaging.SubMaster):
       self.logMonoTime[w] = msg.logMonoTime
 
       self.i += 1
+      if self.i == self.max_i:
+        raise ReplayDone
 
       if w == self.trigger:
         break
diff --git a/selfdrive/test/profiling/locationd.py b/selfdrive/test/profiling/locationd.py
new file mode 100755
index 0000000000..4c478cf778
--- /dev/null
+++ b/selfdrive/test/profiling/locationd.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+
+import cProfile  # pylint: disable=import-error
+import pprofile  # pylint: disable=import-error
+import pyprof2calltree  # pylint: disable=import-error
+
+from tools.lib.logreader import LogReader
+from selfdrive.locationd.locationd import locationd_thread
+from selfdrive.test.profiling.lib import SubMaster, PubMaster, ReplayDone
+
+BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
+
+CARS = {
+  'toyota': ("77611a1fac303767|2020-02-29--13-29-33/3", "TOYOTA COROLLA TSS2 2019"),
+}
+
+
+def get_inputs(msgs, process):
+  sub_socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
+  trigger = 'cameraOdometry'
+
+  sm = SubMaster(msgs, trigger, sub_socks)
+  pm = PubMaster()
+  return sm, pm
+
+
+if __name__ == "__main__":
+  segment, fingerprint = CARS['toyota']
+  segment = segment.replace('|', '/')
+  rlog_url = f"{BASE_URL}{segment}/rlog.bz2"
+  msgs = list(LogReader(rlog_url))
+
+  # Statistical
+  sm, pm = get_inputs(msgs, 'locationd')
+  with pprofile.StatisticalProfile()(period=0.00001) as pr:
+    try:
+      locationd_thread(sm, pm)
+    except ReplayDone:
+      pass
+  pr.dump_stats('cachegrind.out.locationd_statistical')
+
+  # Deterministic
+  sm, pm = get_inputs(msgs, 'controlsd')
+  with cProfile.Profile() as pr:
+    try:
+      locationd_thread(sm, pm)
+    except ReplayDone:
+      pass
+  pyprof2calltree.convert(pr.getstats(), 'cachegrind.out.locationd_deterministic')