Do not run model when skipping frames (#23949)

* Do not run model when skipping frames

* also prepare wide

* add logging

* remove newline

* small skip test

* more random

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/modeld/modeld.cc

@@ -129,11 +129,6 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       vec_desire[desire] = 1.0;
     }
 
-    double mt1 = millis_since_boot();
-    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire);
-    double mt2 = millis_since_boot();
-    float model_execution_time = (mt2 - mt1) / 1000.0;
-
     // tracked dropped frames
     uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
     float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
@@ -144,10 +139,22 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
     run_count++;
 
     float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
+    bool prepare_only = vipc_dropped_frames > 0;
+
+    if (prepare_only) {
+      LOGE("skipping model eval. Dropped %d frames", vipc_dropped_frames);
+    }
 
-    model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
-                  kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
-    posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
+    double mt1 = millis_since_boot();
+    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, prepare_only);
+    double mt2 = millis_since_boot();
+    float model_execution_time = (mt2 - mt1) / 1000.0;
+
+    if (model_output != nullptr) {
+      model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
+                    kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
+      posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
+    }
 
     //printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
     last = mt1;
@@ -164,8 +171,8 @@ int main(int argc, char **argv) {
     assert(ret == 0);
   }
 
-  bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
-  bool use_extra_client = !main_wide_camera;
+  bool main_wide_camera = Params().getBool("EnableWideCamera");
+  bool use_extra_client = !main_wide_camera;  // set for single camera mode
 
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);

selfdrive/modeld/models/driving.cc

@@ -56,7 +56,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
 }
 
 ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in) {
+                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only) {
 #ifdef DESIRE
   if (desire_in != NULL) {
     for (int i = 1; i < DESIRE_LEN; i++) {
@@ -82,6 +82,11 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
     s->m->addExtra(net_extra_buf, s->wide_frame->buf_size);
     LOGT("Extra image added");
   }
+
+  if (prepare_only) {
+    return nullptr;
+  }
+
   s->m->execute();
   LOGT("Execution finished");
 

selfdrive/modeld/models/driving.h

@@ -268,7 +268,7 @@ struct ModelState {
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in);
+                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only);
 void model_free(ModelState* s);
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,

selfdrive/modeld/test/test_modeld.py

@@ -2,6 +2,7 @@
 import time
 import unittest
 import numpy as np
+import random
 
 import cereal.messaging as messaging
 from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
@@ -26,7 +27,7 @@ class TestModeld(unittest.TestCase):
     self.vipc_server.start_listener()
 
     self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
-    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
+    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan'])
 
     managed_processes['modeld'].start()
     time.sleep(0.2)
@@ -36,22 +37,32 @@ class TestModeld(unittest.TestCase):
     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))
+  def _send_frames(self, frame_id, cams=None):
+    if cams is None:
+      cams = ('roadCameraState', 'wideRoadCameraState')
+
+    cs = None
+    for cam in cams:
+      msg = messaging.new_message(cam)
+      cs = getattr(msg, cam)
+      cs.frameId = frame_id
+      cs.timestampSof = int((frame_id * 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.pm.send(msg.which(), msg)
+      self.vipc_server.send(VIPC_STREAM[msg.which()], IMG_BYTES, cs.frameId,
+                            cs.timestampSof, cs.timestampEof)
+    return cs
 
-      self.sm.update(5000)
-      if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
-        self.sm.update(1000)
+  def _wait(self):
+    self.sm.update(5000)
+    if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
+      self.sm.update(1000)
+
+  def test_modeld(self):
+    for n in range(1, 500):
+      cs = self._send_frames(n)
+      self._wait()
 
       mdl = self.sm['modelV2']
       self.assertEqual(mdl.frameId, n)
@@ -64,8 +75,32 @@ class TestModeld(unittest.TestCase):
       self.assertEqual(odo.frameId, n)
       self.assertEqual(odo.timestampEof, cs.timestampEof)
 
-  def test_skipped_frames(self):
-    pass
+  def test_dropped_frames(self):
+    """
+      modeld should only run on consecutive road frames
+    """
+    frame_id = -1
+    road_frames = list()
+    for n in range(1, 50):
+      if (random.random() < 0.1) and n > 3:
+        cams = random.choice([(), ('wideRoadCameraState', )])
+        self._send_frames(n, cams)
+      else:
+        self._send_frames(n)
+        road_frames.append(n)
+      self._wait()
+
+      if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1:
+        frame_id = road_frames[-1]
+
+      mdl = self.sm['modelV2']
+      odo = self.sm['cameraOdometry']
+      self.assertEqual(mdl.frameId, frame_id)
+      self.assertEqual(mdl.frameIdExtra, frame_id)
+      self.assertEqual(odo.frameId, frame_id)
+      if n != frame_id:
+        self.assertFalse(self.sm.updated['modelV2'])
+        self.assertFalse(self.sm.updated['cameraOdometry'])
 
 
 if __name__ == "__main__":