Improve HITL test robustness (#39153)

openpilot/cereal/messaging/messaging.h

@@ -77,6 +77,7 @@ public:
   PubMaster(const std::vector<const char *> &service_list);
   inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); }
   int send(const char *name, MessageBuilder &msg);
+  inline bool all_readers_updated(const char *name) { return sockets_.at(name)->all_readers_updated(); }
   ~PubMaster();
 
 private:

openpilot/selfdrive/test/setup_device_ci.sh

@@ -61,6 +61,10 @@ sleep infinity
 EOF
 chmod +x "$CONTINUE_PATH"
 
+if pgrep -f launch_chffrplus.sh > /dev/null; then
+  sudo systemctl stop comma
+fi
+
 safe_checkout() {
   # completely clean TEST_DIR
 

openpilot/selfdrive/test/test_onroad.py

@@ -3,6 +3,7 @@
 import math
 import json
 import os
+import re
 import shutil
 import subprocess
 import time
@@ -74,6 +75,8 @@ PROCS = {
   "./pandad": 40.0,
   "openpilot.system.qcomgpsd.qcomgpsd": 1.0,
   "openpilot.common.hardware.comma.modem": 10.0,
+  "openpilot.system.ubloxd.ubloxd": 2.0,
+  "openpilot.system.ubloxd.pigeond": 5.0,
 }
 
 TIMINGS = {
@@ -121,6 +124,9 @@ class TestOnroad(OpenpilotTestCase):
       segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog.zst")), Path(Paths.log_root()).iterdir())
       segs = sorted(segs, key=lambda x: x.stat().st_mtime)
       cls.lr = list(LogReader(os.path.join(segs[-1], "rlog.zst")))
+      cls.msgs = defaultdict(list)
+      for m in cls.lr:
+        cls.msgs[m.which()].append(m)
       cls.ts = msgs_to_time_series(cls.lr)
       return
 
@@ -161,9 +167,10 @@ class TestOnroad(OpenpilotTestCase):
           proc.kill()
 
     cls.lr = list(LogReader(os.path.join(str(segs[0]), "rlog.zst")))
-    st = time.monotonic()
+    cls.msgs = defaultdict(list)
+    for m in cls.lr:
+      cls.msgs[m.which()].append(m)
     cls.ts = msgs_to_time_series(cls.lr)
-    print("msgs to time series", time.monotonic() - st)
     log_path = segs[0]
 
     cls.log_sizes = {}
@@ -171,10 +178,6 @@ class TestOnroad(OpenpilotTestCase):
       assert f.is_file()
       cls.log_sizes[f] = f.stat().st_size / 1e6
 
-    cls.msgs = defaultdict(list)
-    for m in cls.lr:
-      cls.msgs[m.which()].append(m)
-
   def test_service_frequencies(self, subtests):
     for s, msgs in self.msgs.items():
       if s in ('initData', 'sentinel'):
@@ -247,11 +250,15 @@ class TestOnroad(OpenpilotTestCase):
           n = list(x.cmdline)[0]
           plogs_by_proc[n].append(x)
 
+    # only the procs that run on this device, e.g. the GPS daemon and uploader vary
+    all_procs = {p.name for p in self.msgs['managerState'][0].managerState.processes if p.shouldBeRunning}
+    procs = {k: v for k, v in PROCS.items() if re.split(r'[./]', k)[-1] in all_procs}
+
     cpu_ok = True
     dt = (self.msgs['procLog'][-1].logMonoTime - self.msgs['procLog'][0].logMonoTime) / 1e9
     header = ['process', 'usage', 'expected', 'max allowed', 'test result']
     rows = []
-    for proc_name, expected in PROCS.items():
+    for proc_name, expected in procs.items():
 
       error = ""
       usage = 0.
@@ -273,13 +280,12 @@ class TestOnroad(OpenpilotTestCase):
     print(tabulate(rows, header, tablefmt="simple_grid", stralign="center", numalign="center", floatfmt=".2f"))
 
     # Ensure there's no missing procs
-    all_procs = {p.name for p in self.msgs['managerState'][0].managerState.processes if p.shouldBeRunning}
     for p in all_procs:
       with subtests.test(proc=p):
         assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}"
 
     # total CPU check
-    procs_tot = sum([(max(x) if isinstance(x, tuple) else x) for x in PROCS.values()])
+    procs_tot = sum([(max(x) if isinstance(x, tuple) else x) for x in procs.values()])
     with subtests.test(name="total CPU"):
       assert procs_tot < MAX_TOTAL_CPU, "Total CPU budget exceeded"
     print("------------------------------------------------")

openpilot/system/loggerd/encoder/encoder.h

@@ -29,6 +29,8 @@ public:
   virtual void set_bitrate(int bitrate) = 0;
   virtual void request_keyframe() = 0;
 
+  // before the first frame is sent: true once a subscriber (loggerd) is connected
+  bool subscribed() { return pm->all_readers_updated(encoder_info.publish_name); }
   void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
 
 protected:

openpilot/system/loggerd/encoderd.cc

@@ -99,6 +99,8 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
     }
 
     bool lagging = false;
+    bool subscribed = false;
+    int frames_waited = 0;
     while (!do_exit) {
       VisionIpcBufExtra extra;
       VisionBuf* buf = vipc_client.recv(&extra);
@@ -114,6 +116,13 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
       }
       lagging = false;
 
+      // creating our publishers kicked out any subscriber that was already connected (loggerd), and they lose
+      // everything we send until they reconnect, like the first keyframe. so wait for them, but not forever
+      if (!subscribed) {
+        subscribed = std::all_of(encoders.begin(), encoders.end(), [](auto &e) { return e->subscribed(); }) || ++frames_waited > MAIN_FPS * 2;
+        if (!subscribed) continue;
+      }
+
       if (!sync_encoders(s, cam_info.stream_type, extra.frame_id)) {
         continue;
       }

openpilot/tools/lib/github_utils.py

@@ -2,6 +2,8 @@ import base64
 import requests
 from http import HTTPMethod
 
+from openpilot.common.utils import retry
+
 class GithubUtils:
   def __init__(self, api_token, data_token, owner='commaai', api_repo='openpilot', data_repo='ci-artifacts'):
     self.OWNER = owner
@@ -32,6 +34,7 @@ class GithubUtils:
     else:
       return r
 
+  @retry(attempts=5)
   def upload_file(self, bucket, path, file_name):
     with open(path, "rb") as f:
       encoded = base64.b64encode(f.read()).decode()

openpilot/tools/lib/log_time_series.py

@@ -1,48 +1,54 @@
+import capnp
 import numpy as np
+from capnp.lib.capnp import _DynamicEnum
 
 
-def flatten_type_dict(d, sep="/", prefix=None):
-  res = {}
-  if isinstance(d, dict):
-    for key, val in d.items():
-      if prefix is None:
-        res.update(flatten_type_dict(val, prefix=key))
+def _list_to_python(value):
+  return [x.to_dict(verbose=True) if hasattr(x, 'to_dict') else
+          _list_to_python(x) if isinstance(x, (capnp._DynamicListReader, capnp._DynamicListBuilder)) else
+          str(x) if isinstance(x, _DynamicEnum) else x for x in value]
+
+
+class _MessageConverter:
+  """Cache schema field handles and destination columns for one service.
+
+  Read directly into columns instead of building and flattening a dictionary for
+  every message. Union branches are initialized only when they are active.
+  """
+  def __init__(self, schema, group, prefix=""):
+    self.schema = schema
+    self.group = group
+    self.prefix = prefix
+    self.fields = [self._field(name) for name in schema.non_union_fields]
+    self.union_fields = {}
+
+  def _field(self, name):
+    field = self.schema.fields[name]
+    kind = 'struct' if field.proto.which() == 'group' else field.proto.slot.type.which()
+    key = self.prefix + name
+    if kind == 'struct':
+      dest = _MessageConverter(field.schema, self.group, key + '/')
+    else:
+      dest = self.group[key] = [None] * max(0, len(self.group["t"]) - 1)
+    return field, kind, dest
+
+  def append(self, message):
+    fields = self.fields
+    if self.schema.union_fields:
+      name = message.which()
+      if name not in self.union_fields:
+        self.union_fields[name] = self._field(name)
+      fields = [self.union_fields[name], *fields]
+    for field, kind, dest in fields:
+      value = message._get_by_field(field)
+      if kind == 'struct':
+        dest.append(value)
       else:
-        res.update(flatten_type_dict(val, prefix=prefix + sep + key))
-    return res
-  elif isinstance(d, list):
-    return {prefix: np.array(d)}
-  else:
-    return {prefix: d}
-
-
-def get_message_dict(message, typ):
-  valid = message.valid
-  message = message._get(typ)
-  if not hasattr(message, 'to_dict') or typ in ('qcomGnss', 'ubloxGnss'):
-    # TODO: support these
-    #print("skipping", typ)
-    return
-
-  msg_dict = message.to_dict(verbose=True)
-  msg_dict = flatten_type_dict(msg_dict)
-  msg_dict['_valid'] = valid
-  return msg_dict
-
-
-def append_dict(path, t, d, values):
-  if path not in values:
-    group = {}
-    group["t"] = []
-    for k in d:
-      group[k] = []
-    values[path] = group
-  else:
-    group = values[path]
-
-  group["t"].append(t)
-  for k, v in d.items():
-    group[k].append(v)
+        if kind == 'list':
+          value = _list_to_python(value)
+        elif kind == 'enum':
+          value = str(value)
+        dest.append(value)
 
 
 def potentially_ragged_array(arr, dtype=None, **kwargs):
@@ -50,6 +56,8 @@ def potentially_ragged_array(arr, dtype=None, **kwargs):
   try:
     return np.array(arr, dtype=dtype, **kwargs)
   except ValueError:
+    # Preserve array-valued rows when a list field has varying lengths.
+    arr = [np.array(value) if isinstance(value, list) else value for value in arr]
     return np.array(arr, dtype=object, **kwargs)
 
 def msgs_to_time_series(msgs):
@@ -59,13 +67,25 @@ def msgs_to_time_series(msgs):
     in seconds.
   """
   values = {}
+  converters = {}
   for msg in msgs:
     typ = msg.which()
+    if typ in ('qcomGnss', 'ubloxGnss'):
+      continue
+    message = msg._get(typ)
+    if not hasattr(message, 'to_dict'):
+      continue
 
-    tm = msg.logMonoTime / 1.0e9
-    msg_dict = get_message_dict(msg, typ)
-    if msg_dict is not None:
-      append_dict(typ, tm, msg_dict, values)
+    if typ not in values:
+      values[typ] = {"t": [], "_valid": []}
+      converters[typ] = _MessageConverter(message.schema, values[typ])
+    group = values[typ]
+    group["t"].append(msg.logMonoTime / 1.0e9)
+    group["_valid"].append(msg.valid)
+    converters[typ].append(message)
+    for column in group.values():
+      if len(column) < len(group["t"]):
+        column.append(None)
 
   # Sort values by time.
   for group in values.values():