diff --git a/selfdrive/ui/replay/camera.cc b/selfdrive/ui/replay/camera.cc index b7e91b9646..7ff07e3044 100644 --- a/selfdrive/ui/replay/camera.cc +++ b/selfdrive/ui/replay/camera.cc @@ -5,68 +5,83 @@ const int YUV_BUF_COUNT = 50; -CameraServer::CameraServer(std::pair cameras[MAX_CAMERAS]) { - if (cameras) { - for (auto type : ALL_CAMERAS) { - std::tie(cameras_[type].width, cameras_[type].height) = cameras[type]; - } - startVipcServer(); +CameraServer::CameraServer(std::pair camera_size[MAX_CAMERAS]) { + for (auto &cam : cameras_) { + std::tie(cam.width, cam.height) = camera_size[cam.type]; } - camera_thread_ = std::thread(&CameraServer::thread, this); + startVipcServer(); } CameraServer::~CameraServer() { - queue_.push({}); - camera_thread_.join(); + for (auto &cam : cameras_) { + if (cam.thread.joinable()) { + cam.queue.push({}); + cam.thread.join(); + } + } vipc_server_.reset(nullptr); } void CameraServer::startVipcServer() { - std::cout << (vipc_server_ ? "restart" : "start") << " vipc server" << std::endl; vipc_server_.reset(new VisionIpcServer("camerad")); for (auto &cam : cameras_) { if (cam.width > 0 && cam.height > 0) { + std::cout << "camera[" << cam.type << "] frame size " << cam.width << "x" << cam.height << std::endl; vipc_server_->create_buffers(cam.rgb_type, UI_BUF_COUNT, true, cam.width, cam.height); vipc_server_->create_buffers(cam.yuv_type, YUV_BUF_COUNT, false, cam.width, cam.height); + if (!cam.thread.joinable()) { + cam.thread = std::thread(&CameraServer::cameraThread, this, std::ref(cam)); + } } } vipc_server_->start_listener(); } -void CameraServer::thread() { +void CameraServer::cameraThread(Camera &cam) { + auto read_frame = [&](FrameReader *fr, int frame_id) { + VisionBuf *rgb_buf = vipc_server_->get_buffer(cam.rgb_type); + VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.yuv_type); + bool ret = fr->get(frame_id, (uint8_t *)rgb_buf->addr, (uint8_t *)yuv_buf->addr); + return ret ? std::pair{rgb_buf, yuv_buf} : std::pair{nullptr, nullptr}; + }; + while (true) { - const auto [type, fr, eidx] = queue_.pop(); + const auto [fr, eidx] = cam.queue.pop(); if (!fr) break; - auto &cam = cameras_[type]; - // restart vipc server if new camera incoming. - if (cam.width != fr->width || cam.height != fr->height) { - cam.width = fr->width; - cam.height = fr->height; - std::cout << "camera[" << type << "] frame size " << cam.width << "x" << cam.height << std::endl; - startVipcServer(); - } + const int id = eidx.getSegmentId(); + bool prefetched = (id == cam.cached_id && eidx.getSegmentNum() == cam.cached_seg && cam.cached_buf.first && cam.cached_buf.second); + auto [rgb, yuv] = prefetched ? cam.cached_buf : read_frame(fr, id); - // send frame - VisionBuf *rgb_buf = vipc_server_->get_buffer(cam.rgb_type); - VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.yuv_type); - if (fr->get(eidx.getSegmentId(), (uint8_t *)rgb_buf->addr, (uint8_t *)yuv_buf->addr)) { + if (rgb && yuv) { VisionIpcBufExtra extra = { .frame_id = eidx.getFrameId(), .timestamp_sof = eidx.getTimestampSof(), .timestamp_eof = eidx.getTimestampEof(), }; - vipc_server_->send(rgb_buf, &extra, false); - vipc_server_->send(yuv_buf, &extra, false); + vipc_server_->send(rgb, &extra, false); + vipc_server_->send(yuv, &extra, false); } else { - std::cout << "camera[" << type << "] failed to get frame:" << eidx.getSegmentId() << std::endl; + std::cout << "camera[" << cam.type << "] failed to get frame:" << eidx.getSegmentId() << std::endl; } + cam.cached_id = id + 1; + cam.cached_seg = eidx.getSegmentNum(); + cam.cached_buf = read_frame(fr, cam.cached_id); + --publishing_; } } void CameraServer::pushFrame(CameraType type, FrameReader *fr, const cereal::EncodeIndex::Reader &eidx) { + auto &cam = cameras_[type]; + if (cam.width != fr->width || cam.height != fr->height) { + cam.width = fr->width; + cam.height = fr->height; + waitFinish(); + startVipcServer(); + } + ++publishing_; - queue_.push({type, fr, eidx}); + cam.queue.push({fr, eidx}); } diff --git a/selfdrive/ui/replay/camera.h b/selfdrive/ui/replay/camera.h index b57654c186..e813b85d3d 100644 --- a/selfdrive/ui/replay/camera.h +++ b/selfdrive/ui/replay/camera.h @@ -8,7 +8,7 @@ class CameraServer { public: - CameraServer(std::pair cameras[MAX_CAMERAS] = nullptr); + CameraServer(std::pair camera_size[MAX_CAMERAS] = nullptr); ~CameraServer(); void pushFrame(CameraType type, FrameReader* fr, const cereal::EncodeIndex::Reader& eidx); inline void waitFinish() { @@ -16,24 +16,26 @@ public: } protected: - void startVipcServer(); - void thread(); - struct Camera { - VisionStreamType rgb_type; + CameraType type; + VisionStreamType rgb_type; VisionStreamType yuv_type; int width; int height; + std::thread thread; + SafeQueue> queue; + int cached_id = -1; + int cached_seg = -1; + std::pair cached_buf; }; + void startVipcServer(); + void cameraThread(Camera &cam); Camera cameras_[MAX_CAMERAS] = { - {.rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_YUV_BACK}, - {.rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_YUV_FRONT}, - {.rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_YUV_WIDE}, + {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_YUV_BACK}, + {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_YUV_FRONT}, + {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_YUV_WIDE}, }; - std::atomic publishing_ = 0; - std::thread camera_thread_; std::unique_ptr vipc_server_; - SafeQueue> queue_; }; diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index d04c5b0010..5795eae3ea 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -216,13 +216,13 @@ void Replay::startStream(const Segment *cur_segment) { } // start camera server - std::pair cameras[MAX_CAMERAS] = {}; + std::pair camera_size[MAX_CAMERAS] = {}; for (auto type : ALL_CAMERAS) { if (auto &fr = cur_segment->frames[type]) { - cameras[type] = {fr->width, fr->height}; + camera_size[type] = {fr->width, fr->height}; } } - camera_server_ = std::make_unique(cameras); + camera_server_ = std::make_unique(camera_size); // start stream thread stream_thread_ = QThread::create(&Replay::stream, this);