diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 03dd4f4686..0c04a0e9e9 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.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 && \ diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index dd0bafff31..639acd2d22 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -51,10 +51,6 @@ mat3 update_calibration(Eigen::Matrix &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); diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py new file mode 100755 index 0000000000..93ad0f5608 --- /dev/null +++ b/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()