test: Replay both models on C2/C3 (#23019)

* add dcam stream

* delete model_test

* both in jenkins

* update spinner

* get filename fn

* they are not random names

* update route

* new ref commit

* temporarily resize tmp

* clean up unlog send

* need calib 0

* revert device

* cleanup

* arg

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 1ec8f03ec642fb5117727eaf31df4841625e0d7b

Jenkinsfile

@@ -247,6 +247,15 @@ pipeline {
                   }
                 }
 
+                stage('C3: replay') {
+                  steps {
+                    phone_steps("tici-party", [
+                      ["build", "cd selfdrive/manager && ./build.py"],
+                      ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"],
+                    ])
+                  }
+                }
+
               }
             }
 

selfdrive/test/model_test.py

@@ -1,30 +0,0 @@
-#!/usr/bin/env python3
-import os
-import numpy as np
-
-from tools.lib.logreader import LogReader
-from tools.lib.framereader import FrameReader
-from tools.lib.cache import cache_path_for_file_path
-from selfdrive.test.process_replay.model_replay import model_replay
-
-
-if __name__ == "__main__":
-  lr = LogReader(os.path.expanduser('~/rlog.bz2'))
-  fr = FrameReader(os.path.expanduser('~/fcamera.hevc'))
-  desire = np.load(os.path.expanduser('~/desire.npy'))
-  calib = np.load(os.path.expanduser('~/calib.npy'))
-
-  try:
-    msgs = model_replay(list(lr), fr, desire=desire, calib=calib)
-  finally:
-    cache_path = cache_path_for_file_path(os.path.expanduser('~/fcamera.hevc'))
-    if os.path.isfile(cache_path):
-      os.remove(cache_path)
-
-  output_size = len(np.frombuffer(msgs[0].model.rawPredictions, dtype=np.float32))
-  output_data = np.zeros((len(msgs), output_size), dtype=np.float32)
-  for i, msg in enumerate(msgs):
-    output_data[i] = np.frombuffer(msg.model.rawPredictions, dtype=np.float32)
-  np.save(os.path.expanduser('~/modeldata.npy'), output_data)
-
-  print("Finished replay")

selfdrive/test/process_replay/model_replay.py

@@ -7,12 +7,12 @@ from typing import Any
 from tqdm import tqdm
 
 import cereal.messaging as messaging
-from cereal import log
 from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
 from common.spinner import Spinner
 from common.timeout import Timeout
-from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size
-from selfdrive.hardware import PC
+from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \
+                                                                        eon_d_frame_size, tici_d_frame_size
+from selfdrive.hardware import PC, TICI
 from selfdrive.manager.process_config import managed_processes
 from selfdrive.test.openpilotci import BASE_URL, get_url
 from selfdrive.test.process_replay.compare_logs import compare_logs, save_log
@@ -21,8 +21,17 @@ from selfdrive.version import get_git_commit
 from tools.lib.framereader import FrameReader
 from tools.lib.logreader import LogReader
 
-TEST_ROUTE = "99c94dc769b5d96e|2019-08-03--14-19-59"
+if TICI:
+  TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
+else:
+  TEST_ROUTE = "303055c0002aefd1|2021-11-22--18-36-32"
 
+CACHE_DIR = os.getenv("CACHE_DIR", None)
+
+packet_from_camera = {"roadCameraState":"modelV2", "driverCameraState":"driverState"}
+
+def get_log_fn(ref_commit):
+  return "%s_%s_%s.bz2" % (TEST_ROUTE, "model_tici" if TICI else "model", ref_commit)
 
 def replace_calib(msg, calib):
   msg = msg.as_builder()
@@ -30,67 +39,78 @@ def replace_calib(msg, calib):
     msg.liveCalibration.extrinsicMatrix = get_view_frame_from_road_frame(*calib, 1.22).flatten().tolist()
   return msg
 
+def process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire):
+  if msg.which() == "roadCameraState" and last_desire is not None:
+    dat = messaging.new_message('lateralPlan')
+    dat.lateralPlan.desire = last_desire
+    pm.send('lateralPlan', dat)
 
-def model_replay(lr, fr, desire=None, calib=None):
+  f = msg.as_builder()
+  pm.send(msg.which(), f)
 
+  img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0]
+  if msg.which == "roadCameraState":
+    vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK, img.flatten().tobytes(), f.roadCameraState.frameId,
+                      f.roadCameraState.timestampSof, f.roadCameraState.timestampEof)
+  else:
+    vipc_server.send(VisionStreamType.VISION_STREAM_YUV_FRONT, img.flatten().tobytes(), f.driverCameraState.frameId,
+                      f.driverCameraState.timestampSof, f.driverCameraState.timestampEof)
+  with Timeout(seconds=15):
+    log_msgs.append(messaging.recv_one(sm.sock[packet_from_camera[msg.which()]]))
+
+  frame_idxs[msg.which()] += 1
+  if frame_idxs[msg.which()] >= frs[msg.which()].frame_count:
+    return None
+  update_spinner(spinner, frame_idxs['roadCameraState'], frs['roadCameraState'].frame_count,
+                                         frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)
+  return 0
+
+def update_spinner(s, fidx, fcnt, didx, dcnt):
+  s.update("replaying models:  road %d/%d,  driver %d/%d" % (fidx, fcnt, didx, dcnt))
+
+def model_replay(lr_list, frs):
   spinner = Spinner()
   spinner.update("starting model replay")
 
-  vipc_server = None
-  pm = messaging.PubMaster(['roadCameraState', 'liveCalibration', 'lateralPlan'])
-  sm = messaging.SubMaster(['modelV2'])
+  vipc_server = VisionIpcServer("camerad")
+  vipc_server.create_buffers(VisionStreamType.VISION_STREAM_YUV_BACK, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size))
+  vipc_server.create_buffers(VisionStreamType.VISION_STREAM_YUV_FRONT, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size))
+  vipc_server.start_listener()
+
+  pm = messaging.PubMaster(['roadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
+  sm = messaging.SubMaster(['modelV2', 'driverState'])
 
-  # TODO: add dmonitoringmodeld
   try:
     managed_processes['modeld'].start()
+    managed_processes['dmonitoringmodeld'].start()
     time.sleep(5)
     sm.update(1000)
 
-    desires_by_index = {v:k for k,v in log.LateralPlan.Desire.schema.enumerants.items()}
+    last_desire = None
+    log_msgs = []
+    frame_idxs = dict.fromkeys(['roadCameraState','driverCameraState'], 0)
 
     cal = [msg for msg in lr if msg.which() == "liveCalibration"]
     for msg in cal[:5]:
-      pm.send(msg.which(), replace_calib(msg, calib))
+      pm.send(msg.which(), replace_calib(msg, None))
 
-    log_msgs = []
-    frame_idx = 0
-    for msg in tqdm(lr):
+    for msg in tqdm(lr_list):
       if msg.which() == "liveCalibration":
-        pm.send(msg.which(), replace_calib(msg, calib))
-      elif msg.which() == "roadCameraState":
-        if desire is not None:
-          for i in desire[frame_idx].nonzero()[0]:
-            dat = messaging.new_message('lateralPlan')
-            dat.lateralPlan.desire = desires_by_index[i]
-            pm.send('lateralPlan', dat)
-
-        f = msg.as_builder()
-        pm.send(msg.which(), f)
-
-        img = fr.get(frame_idx, pix_fmt="yuv420p")[0]
-        if vipc_server is None:
-          w, h = {int(3*w*h/2): (w, h) for (w, h) in [tici_f_frame_size, eon_f_frame_size]}[len(img)]
-          vipc_server = VisionIpcServer("camerad")
-          vipc_server.create_buffers(VisionStreamType.VISION_STREAM_YUV_BACK, 40, False, w, h)
-          vipc_server.start_listener()
-          time.sleep(1) # wait for modeld to connect
-
-        vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK, img.flatten().tobytes(), f.roadCameraState.frameId,
-                         f.roadCameraState.timestampSof, f.roadCameraState.timestampEof)
-
-        with Timeout(seconds=15):
-          log_msgs.append(messaging.recv_one(sm.sock['modelV2']))
-
-        spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count))
-
-        frame_idx += 1
-        if frame_idx >= fr.frame_count:
+        last_calib = list(msg.liveCalibration.rpyCalib)
+        pm.send(msg.which(), replace_calib(msg, last_calib))
+      elif msg.which() == "lateralPlan":
+        last_desire = msg.lateralPlan.desire
+      elif msg.which() in ["roadCameraState", "driverCameraState"]:
+        ret = process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire)
+        if ret is None:
           break
+
   except KeyboardInterrupt:
     pass
   finally:
     spinner.close()
     managed_processes['modeld'].stop()
+    managed_processes['dmonitoringmodeld'].stop()
 
   return log_msgs
 
@@ -98,26 +118,40 @@ if __name__ == "__main__":
 
   update = "--update" in sys.argv
 
+  if TICI:
+    os.system('sudo mount -o remount,size=200M /tmp') # c3 hevcs are 75M each
+
   replay_dir = os.path.dirname(os.path.abspath(__file__))
   ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
 
-  lr = LogReader(get_url(TEST_ROUTE, 0))
-  fr = FrameReader(get_url(TEST_ROUTE, 0, log_type="fcamera"))
+  segnum = 0
+  frs = {}
+  if CACHE_DIR:
+    lr = LogReader(os.path.join(CACHE_DIR, '%s--%d--rlog.bz2' % (TEST_ROUTE.replace('|', '_'), segnum)))
+    frs['roadCameraState'] = FrameReader(os.path.join(CACHE_DIR, '%s--%d--fcamera.hevc' % (TEST_ROUTE.replace('|', '_'), segnum)))
+    frs['driverCameraState'] = FrameReader(os.path.join(CACHE_DIR, '%s--%d--dcamera.hevc' % (TEST_ROUTE.replace('|', '_'), segnum)))
+  else:
+    lr = LogReader(get_url(TEST_ROUTE, segnum))
+    frs['roadCameraState'] = FrameReader(get_url(TEST_ROUTE, segnum, log_type="fcamera"))
+    frs['driverCameraState'] = FrameReader(get_url(TEST_ROUTE, segnum, log_type="dcamera"))
 
-  log_msgs = model_replay(list(lr), fr)
+  lr_list = list(lr)
+  log_msgs = model_replay(lr_list, frs)
 
   failed = False
   if not update:
     ref_commit = open(ref_commit_fn).read().strip()
-    log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit)
+    log_fn = get_log_fn(ref_commit)
     cmp_log = LogReader(BASE_URL + log_fn)
 
     ignore = ['logMonoTime', 'valid',
               'modelV2.frameDropPerc',
-              'modelV2.modelExecutionTime']
+              'modelV2.modelExecutionTime',
+              'driverState.modelExecutionTime',
+              'driverState.dspExecutionTime']
     tolerance = None if not PC else 1e-3
     results: Any = {TEST_ROUTE: {}}
-    results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore)
+    results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore)
     diff1, diff2, failed = format_diff(results, ref_commit)
 
     print(diff2)
@@ -136,7 +170,7 @@ if __name__ == "__main__":
     print("Uploading new refs")
 
     new_commit = get_git_commit()
-    log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", new_commit)
+    log_fn = get_log_fn(new_commit)
     save_log(log_fn, log_msgs)
     try:
       upload_file(log_fn, os.path.basename(log_fn))

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-46abfc14629d16f84ee45255ac876dceea2e7a11
+c1f0accb010eea2d524427a5264055778063f464