diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 047e1bd45d..0fed108045 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -17,6 +17,9 @@ const int NUM_VIPC_BUFFERS = 4; const int EARTH_CIRCUMFERENCE_METERS = 40075000; const int PIXELS_PER_TILE = 256; +const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); +const int LLK_DECIMATION = TEST_MODE ? 1 : 10; + float get_zoom_level_for_scale(float lat, float meters_per_pixel) { float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE; float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile; @@ -89,7 +92,7 @@ void MapRenderer::msgUpdate() { auto orientation = location.getCalibratedOrientationNED(); bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); - if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % 10) == 0) { + if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2])); // TODO: use the static rendering mode @@ -151,6 +154,15 @@ void MapRenderer::update() { } } +void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array &buf) { + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initNavThumbnail(); + thumbnaild.setFrameId(frame_id); + thumbnaild.setTimestampEof(ts); + thumbnaild.setThumbnail(buf); + pm->send("navThumbnail", msg); +} + void MapRenderer::publish(const double render_time) { QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); uint64_t ts = nanos_since_boot(); @@ -173,7 +185,12 @@ void MapRenderer::publish(const double render_time) { vipc_server->send(buf, &extra); - if (frame_id % 100 == 0) { + // Send thumbnail + if (TEST_MODE) { + // Full image in thumbnails in test mode + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)cap.bits(), cap.sizeInBytes()); + sendThumbnail(ts, buffer_kj); + } else if (frame_id % 100 == 0) { // Write jpeg into buffer QByteArray buffer_bytes; QBuffer buffer(&buffer_bytes); @@ -181,14 +198,7 @@ void MapRenderer::publish(const double render_time) { cap.save(&buffer, "JPG", 50); kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); - - // Send thumbnail - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initNavThumbnail(); - thumbnaild.setFrameId(frame_id); - thumbnaild.setTimestampEof(ts); - thumbnaild.setThumbnail(buffer_kj); - pm->send("navThumbnail", msg); + sendThumbnail(ts, buffer_kj); } // Send state msg diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index 1c7024361d..0019030b6d 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -35,6 +35,7 @@ private: std::unique_ptr pm; std::unique_ptr sm; void publish(const double render_time); + void sendThumbnail(const uint64_t ts, const kj::Array &buf); QMapboxGLSettings m_settings; QScopedPointer m_map; diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index e5e21a6fef..c96e624421 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -80,7 +80,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non diff = [] for msg1, msg2 in zip(log1, log2): if msg1.which() != msg2.which(): - print(msg1, msg2) + print(msg1.which(), msg2.which()) raise Exception("msgs not aligned between logs") msg1 = remove_ignored_fields(msg1, ignore_fields) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 1f43bb791a..ce861b37e6 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30" SEGMENT = 6 MAX_FRAMES = 100 if PC else 600 -NAV_FRAMES = 20 +NAV_FRAMES = 50 NO_NAV = "NO_NAV" in os.environ # TODO: make map renderer work in CI SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) @@ -43,39 +43,46 @@ def replace_calib(msg, calib): def nav_model_replay(lr): + sm = messaging.SubMaster(['navModel', 'navThumbnail']) pm = messaging.PubMaster(['liveLocationKalman', 'navRoute']) - sock = messaging.sub_sock('navModel', conflate=False, timeout=1000) + + nav = [m for m in lr if m.which() == 'navRoute'] + llk = [m for m in lr if m.which() == 'liveLocationKalman'] + assert len(nav) > 0 and len(llk) >= NAV_FRAMES log_msgs = [] try: + assert "MAPBOX_TOKEN" in os.environ + os.environ['MAP_RENDER_TEST_MODE'] = '1' managed_processes['mapsd'].start() managed_processes['navmodeld'].start() # setup position and route - nav = [m for m in lr if m.which() == 'navRoute'] - llk = [m for m in lr if m.which() == 'liveLocationKalman'] - assert len(nav) > 0 and len(llk) > 0 - - for _ in range(30): + for _ in range(10): for s in (llk, nav): - msg = s[0] - pm.send(msg.which(), msg.as_builder().to_bytes()) - if messaging.recv_one(sock) is not None: + pm.send(s[0].which(), s[0].as_builder().to_bytes()) + sm.update(1000) + if sm.updated['navModel']: break - else: - raise Exception("no navmodeld outputs") + time.sleep(1) + + if not sm.updated['navModel']: + raise Exception("no navmodeld outputs, failed to initialize") + # drain time.sleep(2) - messaging.drain_sock_raw(sock) + sm.update(0) # run replay - for _ in range(NAV_FRAMES): - # 2Hz decimation - for _ in range(10): - pm.send(llk[0].which(), llk[0].as_builder().to_bytes()) - time.sleep(0.1) - with Timeout(5, "timed out waiting for nav model outputs"): - log_msgs.append(messaging.recv_one_retry(sock)) + for n in range(NAV_FRAMES): + pm.send(llk[n].which(), llk[n].as_builder().to_bytes()) + m = messaging.recv_one(sm.sock['navThumbnail']) + assert m is not None, f"no navThumbnail, frame={n}" + log_msgs.append(m) + + m = messaging.recv_one(sm.sock['navModel']) + assert m is not None, f"no navModel response, frame={n}" + log_msgs.append(m) finally: managed_processes['mapsd'].stop() managed_processes['navmodeld'].stop() @@ -212,7 +219,7 @@ if __name__ == "__main__": try: expected_msgs = 2*MAX_FRAMES if not NO_NAV: - expected_msgs += NAV_FRAMES + expected_msgs += NAV_FRAMES*2 cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs] ignore = [ @@ -223,6 +230,7 @@ if __name__ == "__main__": 'driverStateV2.dspExecutionTime', 'navModel.dspExecutionTime', 'navModel.modelExecutionTime', + 'navThumbnail.timestampEof', ] if PC: ignore += [ diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index b58cca1aaf..4dcf0ecbb7 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -4b2c6516cd460ee443b9006f01233168edf3d170 \ No newline at end of file +30dfeed50ac562f99b0aad985bd6b0e6d8188fcb \ No newline at end of file