modeld tests (#24263)

.github/workflows/selfdrive_tests.yaml

@@ -257,6 +257,7 @@ jobs:
                         $UNIT_TEST selfdrive/athena && \
                         $UNIT_TEST selfdrive/thermald && \
                         $UNIT_TEST selfdrive/hardware/tici && \
+                        $UNIT_TEST selfdrive/modeld && \
                         $UNIT_TEST tools/lib/tests && \
                         ./selfdrive/boardd/tests/test_boardd_usbprotocol && \
                         ./selfdrive/common/tests/test_util && \

selfdrive/modeld/modeld.cc

@@ -51,10 +51,6 @@ mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera
   return matmul3(yuv_transform, transform);
 }
 
-static uint64_t get_ts(const VisionIpcBufExtra &extra) {
-  return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof;
-}
-
 
 void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
   // messaging
@@ -80,7 +76,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
 
   while (!do_exit) {
     // Keep receiving frames until we are at least 1 frame ahead of previous extra frame
-    while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) {
+    while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) {
       buf_main = vipc_client_main.recv(&meta_main);
       if (buf_main == nullptr)  break;
     }
@@ -94,7 +90,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       // Keep receiving extra frames until frame id matches main camera
       do {
         buf_extra = vipc_client_extra.recv(&meta_extra);
-      } while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
+      } while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL);
 
       if (buf_extra == nullptr) {
         LOGE("vipc_client_extra no frame");
@@ -124,7 +120,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       }
 
       model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false);
-      model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
+      model_transform_extra = update_calibration(extrinsic_matrix_eigen, true, true);
       live_calib_seen = true;
     }
 
@@ -169,7 +165,7 @@ int main(int argc, char **argv) {
   }
 
   bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
-  bool use_extra_client = Hardware::TICI() && !main_wide_camera;
+  bool use_extra_client = !main_wide_camera;
 
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);

selfdrive/modeld/test/test_modeld.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+import time
+import unittest
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from common.transformations.camera import tici_f_frame_size
+from common.realtime import DT_MDL
+from selfdrive.manager.process_config import managed_processes
+
+
+VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
+               "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD}
+
+IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8)
+IMG_BYTES = IMG.flatten().tobytes()
+
+class TestModeld(unittest.TestCase):
+
+  def setUp(self):
+    self.vipc_server = VisionIpcServer("camerad")
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
+    self.vipc_server.start_listener()
+
+    self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
+    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
+
+    managed_processes['modeld'].start()
+    time.sleep(0.2)
+    self.sm.update(1000)
+
+  def tearDown(self):
+    managed_processes['modeld'].stop()
+    del self.vipc_server
+
+  def test_modeld(self):
+    for n in range(1, 500):
+      for cam in ('roadCameraState', 'wideRoadCameraState'):
+        msg = messaging.new_message(cam)
+        cs = getattr(msg, cam)
+        cs.frameId = n
+        cs.timestampSof = int((n * DT_MDL) * 1e9)
+        cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9))
+
+        self.pm.send(msg.which(), msg)
+        self.vipc_server.send(VIPC_STREAM[msg.which()], IMG_BYTES, cs.frameId,
+                              cs.timestampSof, cs.timestampEof)
+
+      self.sm.update(5000)
+      if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
+        self.sm.update(1000)
+
+      mdl = self.sm['modelV2']
+      self.assertEqual(mdl.frameId, n)
+      self.assertEqual(mdl.frameIdExtra, n)
+      self.assertEqual(mdl.timestampEof, cs.timestampEof)
+      self.assertEqual(mdl.frameAge, 0)
+      self.assertEqual(mdl.frameDropPerc, 0)
+
+      odo = self.sm['cameraOdometry']
+      self.assertEqual(odo.frameId, n)
+      self.assertEqual(odo.timestampEof, cs.timestampEof)
+
+  def test_skipped_frames(self):
+    pass
+
+
+if __name__ == "__main__":
+  unittest.main()