From 374190873349fc9cb59771dbd60ca1a759ff3b8c Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 22 Apr 2022 22:59:49 +0200 Subject: [PATCH] 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 --- selfdrive/modeld/modeld.cc | 27 +++++++---- selfdrive/modeld/models/driving.cc | 7 ++- selfdrive/modeld/models/driving.h | 2 +- selfdrive/modeld/test/test_modeld.py | 69 +++++++++++++++++++++------- 4 files changed, 76 insertions(+), 29 deletions(-) diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 639acd2d22fe1a..6ee07b3d021dd4 100644 --- a/selfdrive/modeld/modeld.cc +++ b/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(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(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); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 422ad21cdeb530..e14411df787b1f 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/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"); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index f83cb869392bb8..47dec0d9d740d5 100644 --- a/selfdrive/modeld/models/driving.h +++ b/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, diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py index 93ad0f5608f94f..f09aec578c94fb 100755 --- a/selfdrive/modeld/test/test_modeld.py +++ b/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 @@ def setUp(self): 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 @@ 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)) + 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 @@ def test_modeld(self): 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__":