From e6478c6b35db1707cebe21a75df9ffa816b6c313 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Thu, 14 Jan 2021 17:49:17 -0800
Subject: [PATCH] better can replay script

---
 selfdrive/boardd/tests/replay_many.py  | 83 --------------------------
 selfdrive/debug/internal/can_replay.py | 81 +++++++++++++++++++++++++
 2 files changed, 81 insertions(+), 83 deletions(-)
 delete mode 100755 selfdrive/boardd/tests/replay_many.py
 create mode 100755 selfdrive/debug/internal/can_replay.py

diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py
deleted file mode 100755
index 71db229a21..0000000000
--- a/selfdrive/boardd/tests/replay_many.py
+++ /dev/null
@@ -1,83 +0,0 @@
-#!/usr/bin/env python3
-import os
-import sys
-import time
-import signal
-import traceback
-import usb1
-from panda import Panda, PandaDFU
-from multiprocessing import Pool
-
-jungle = "JUNGLE" in os.environ
-if jungle:
-  from panda_jungle import PandaJungle  # pylint: disable=import-error
-
-import cereal.messaging as messaging
-from selfdrive.boardd.boardd import can_capnp_to_can_list
-
-def initializer():
-  """Ignore CTRL+C in the worker process.
-  source: https://stackoverflow.com/a/44869451 """
-  signal.signal(signal.SIGINT, signal.SIG_IGN)
-
-def send_thread(sender_serial):
-  global jungle
-  while True:
-    try:
-      if jungle:
-        sender = PandaJungle(sender_serial)
-      else:
-        sender = Panda(sender_serial)
-        sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
-
-      sender.set_can_loopback(False)
-      can_sock = messaging.sub_sock('can')
-
-      while True:
-        tsc = messaging.recv_one(can_sock)
-        snd = can_capnp_to_can_list(tsc.can)
-        snd = list(filter(lambda x: x[-1] <= 2, snd))
-
-        try:
-          sender.can_send_many(snd)
-        except usb1.USBErrorTimeout:
-          pass
-
-        # Drain panda message buffer
-        sender.can_recv()
-    except Exception:
-      traceback.print_exc()
-      time.sleep(1)
-
-if __name__ == "__main__":
-  if jungle:
-    serials = PandaJungle.list()
-  else:
-    serials = Panda.list()
-  num_senders = len(serials)
-
-  if num_senders == 0:
-    print("No senders found. Exiting")
-    sys.exit(1)
-  else:
-    print("%d senders found. Starting broadcast" % num_senders)
-
-  if "FLASH" in os.environ:
-    for s in PandaDFU.list():
-      PandaDFU(s).recover()
-
-    time.sleep(1)
-    for s in serials:
-      Panda(s).recover()
-      Panda(s).flash()
-
-  pool = Pool(num_senders, initializer=initializer)
-  pool.map_async(send_thread, serials)
-
-  while True:
-    try:
-      time.sleep(10)
-    except KeyboardInterrupt:
-      pool.terminate()
-      pool.join()
-      raise
diff --git a/selfdrive/debug/internal/can_replay.py b/selfdrive/debug/internal/can_replay.py
new file mode 100755
index 0000000000..683b68e965
--- /dev/null
+++ b/selfdrive/debug/internal/can_replay.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+import os
+import time
+from multiprocessing import Process
+from tqdm import tqdm
+
+os.environ['FILEREADER_CACHE'] = '1'
+
+from common.realtime import config_realtime_process, Ratekeeper
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+from tools.lib.logreader import LogReader
+
+from panda import Panda
+try:
+  from panda_jungle import PandaJungle
+except Exception:
+  PandaJungle = None # type: ignore
+
+
+ROUTE = "77611a1fac303767/2020-03-24--09-50-38"
+NUM_SEGS = 10 # route has 82 segments available
+
+print("Loading log...")
+CAN_MSGS = []
+for i in tqdm(list(range(1, NUM_SEGS))):
+  log_url = f"https://commadataci.blob.core.windows.net/openpilotci/{ROUTE}/{i}/rlog.bz2"
+  lr = LogReader(log_url)
+  CAN_MSGS += [can_capnp_to_can_list(m.can) for m in lr if m.which() == 'can']
+
+def send_thread(sender, core):
+  config_realtime_process(core, 55)
+
+  if "Jungle" in str(type(sender)):
+    sender.set_ignition(False)
+    time.sleep(3)
+    sender.set_ignition(True)
+  else:
+    sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  sender.set_can_loopback(False)
+
+  log_idx = 0
+  rk = Ratekeeper(100)
+  while True:
+    snd = CAN_MSGS[log_idx]
+    log_idx = (log_idx + 1) % len(CAN_MSGS)
+    snd = list(filter(lambda x: x[-1] <= 2, snd))
+    sender.can_send_many(snd)
+
+    # Drain panda message buffer
+    sender.can_recv()
+    rk.keep_time()
+
+def connect():
+  serials = {}
+  while True:
+    # look for new devices
+    for p in [Panda, PandaJungle]:
+      if p is None:
+        continue
+
+      for s in p.list():
+        if s not in serials:
+          print("starting send thread for", s)
+          serials[s] = Process(target=send_thread, args=(p(s), 3))
+          serials[s].start()
+
+    # try to join all send procs
+    cur_serials = serials.copy()
+    for s, p in cur_serials.items():
+      p.join(0.01)
+      if p.exitcode is not None:
+        del serials[s]
+
+    time.sleep(1)
+
+if __name__ == "__main__":
+  while True:
+    try:
+      connect()
+    except Exception:
+      pass