diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index c8c1a34375..9cca0a35d4 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -26,7 +26,7 @@ PROCS = { "./camerad": 41.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 18.4, + "./_ui": 33.0, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 3302687bf4..c17d2e2573 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -12,7 +12,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { layout = new QStackedLayout(this); layout->setStackingMode(QStackedLayout::StackAll); - cameraView = new CameraViewWidget("camerad", VISION_STREAM_DRIVER, true, this); + cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, true, this); layout->addWidget(cameraView); scene = new DriverViewScene(this); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 9fd365d2e2..af795014ef 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -20,7 +20,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { QStackedLayout *road_view_layout = new QStackedLayout; road_view_layout->setStackingMode(QStackedLayout::StackAll); - nvg = new NvgWindow(VISION_STREAM_ROAD, this); + nvg = new NvgWindow(VISION_STREAM_RGB_ROAD, this); road_view_layout->addWidget(nvg); hud = new OnroadHud(this); road_view_layout->addWidget(hud); @@ -97,7 +97,7 @@ void OnroadWindow::offroadTransition(bool offroad) { // update stream type bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera"); - nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); + nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE_ROAD : VISION_STREAM_RGB_ROAD); } void OnroadWindow::paintEvent(QPaintEvent *event) { diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 3b773bd874..d5069819e7 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -17,10 +17,10 @@ const char frame_vertex_shader[] = #else "#version 300 es\n" #endif - "layout(location = 0) in vec4 aPosition;\n" - "layout(location = 1) in vec2 aTexCoord;\n" + "in vec4 aPosition;\n" + "in vec4 aTexCoord;\n" "uniform mat4 uTransform;\n" - "out vec2 vTexCoord;\n" + "out vec4 vTexCoord;\n" "void main() {\n" " gl_Position = uTransform * aPosition;\n" " vTexCoord = aTexCoord;\n" @@ -33,19 +33,11 @@ const char frame_fragment_shader[] = "#version 300 es\n" "precision mediump float;\n" #endif - "uniform sampler2D uTextureY;\n" - "uniform sampler2D uTextureU;\n" - "uniform sampler2D uTextureV;\n" - "in vec2 vTexCoord;\n" + "uniform sampler2D uTexture;\n" + "in vec4 vTexCoord;\n" "out vec4 colorOut;\n" "void main() {\n" - " float y = texture(uTextureY, vTexCoord).r;\n" - " float u = texture(uTextureU, vTexCoord).r - 0.5;\n" - " float v = texture(uTextureV, vTexCoord).r - 0.5;\n" - " float r = y + 1.402 * v;\n" - " float g = y - 0.344 * u - 0.714 * v;\n" - " float b = y + 1.772 * u;\n" - " colorOut = vec4(r, g, b, 1.0);\n" + " colorOut = texture(uTexture, vTexCoord.xy);\n" "}\n"; const mat4 device_transform = {{ @@ -102,10 +94,6 @@ CameraViewWidget::CameraViewWidget(std::string stream_name, VisionStreamType typ stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection); - connect(this, &CameraViewWidget::vipcThreadFrameReceived, [=](VisionBuf *buf) { - latest_frame = buf; - update(); - }); } CameraViewWidget::~CameraViewWidget() { @@ -114,7 +102,6 @@ CameraViewWidget::~CameraViewWidget() { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); glDeleteBuffers(1, &frame_ibo); - glDeleteBuffers(3, textures); } doneCurrent(); } @@ -132,7 +119,7 @@ void CameraViewWidget::initializeGL() { GLint frame_pos_loc = program->attributeLocation("aPosition"); GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); - auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; const float frame_coords[4][4] = { {-1.0, -1.0, x2, y1}, // bl @@ -157,16 +144,10 @@ void CameraViewWidget::initializeGL() { glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); glBindBuffer(GL_ARRAY_BUFFER, 0); glBindVertexArray(0); - - glGenTextures(3, textures); - glUseProgram(program->programId()); - glUniform1i(program->uniformLocation("uTextureY"), 0); - glUniform1i(program->uniformLocation("uTextureU"), 1); - glUniform1i(program->uniformLocation("uTextureV"), 2); } void CameraViewWidget::showEvent(QShowEvent *event) { - latest_frame = nullptr; + latest_texture_id = -1; if (!vipc_thread) { vipc_thread = new QThread(); connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); @@ -186,12 +167,12 @@ void CameraViewWidget::hideEvent(QHideEvent *event) { void CameraViewWidget::updateFrameMat(int w, int h) { if (zoomed_view) { - if (stream_type == VISION_STREAM_DRIVER) { + if (stream_type == VISION_STREAM_RGB_DRIVER) { frame_mat = matmul(device_transform, get_driver_view_transform(w, h, stream_width, stream_height)); } else { - auto intrinsic_matrix = stream_type == VISION_STREAM_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; float zoom = ZOOM / intrinsic_matrix.v[0]; - if (stream_type == VISION_STREAM_WIDE_ROAD) { + if (stream_type == VISION_STREAM_RGB_WIDE_ROAD) { zoom *= 0.5; } float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); @@ -219,7 +200,7 @@ void CameraViewWidget::paintGL() { std::lock_guard lk(lock); - if (latest_frame == nullptr) return; + if (latest_texture_id == -1) return; glViewport(0, 0, width(), height()); // sync with the PBO @@ -228,21 +209,13 @@ void CameraViewWidget::paintGL() { } glBindVertexArray(frame_vao); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, texture[latest_texture_id]->frame_tex); glUseProgram(program->programId()); - uint8_t *address[3] = {latest_frame->y, latest_frame->u, latest_frame->v}; - for (int i = 0; i < 3; ++i) { - glActiveTexture(GL_TEXTURE0 + i); - glBindTexture(GL_TEXTURE_2D, textures[i]); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_LUMINANCE, GL_UNSIGNED_BYTE, address[i]); - assert(glGetError() == GL_NO_ERROR); - } - - glActiveTexture(GL_TEXTURE0); // qt requires active texture 0 - + glUniform1i(program->uniformLocation("uTexture"), 0); glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); + assert(glGetError() == GL_NO_ERROR); glEnableVertexAttribArray(0); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); @@ -252,23 +225,22 @@ void CameraViewWidget::paintGL() { void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { makeCurrent(); - latest_frame = nullptr; - stream_width = vipc_client->buffers[0].width; - stream_height = vipc_client->buffers[0].height; + for (int i = 0; i < vipc_client->num_buffers; i++) { + texture[i].reset(new EGLImageTexture(&vipc_client->buffers[i])); + + glBindTexture(GL_TEXTURE_2D, texture[i]->frame_tex); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); - for (int i = 0; i < 3; ++i) { - glBindTexture(GL_TEXTURE_2D, textures[i]); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - int width = i == 0 ? stream_width : stream_width / 2; - int height = i == 0 ? stream_height : stream_height / 2; - glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, nullptr); + // BGR + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); assert(glGetError() == GL_NO_ERROR); } - + latest_texture_id = -1; + stream_width = vipc_client->buffers[0].width; + stream_height = vipc_client->buffers[0].height; updateFrameMat(width(), height()); } @@ -276,6 +248,23 @@ void CameraViewWidget::vipcThread() { VisionStreamType cur_stream_type = stream_type; std::unique_ptr vipc_client; + std::unique_ptr ctx; + std::unique_ptr surface; + std::unique_ptr gl_buffer; + + ctx = std::make_unique(); + ctx->setFormat(context()->format()); + ctx->setShareContext(context()); + ctx->create(); + assert(ctx->isValid()); + + surface = std::make_unique(); + surface->setFormat(ctx->format()); + surface->create(); + ctx->makeCurrent(surface.get()); + assert(QOpenGLContext::currentContext() == ctx.get()); + initializeOpenGLFunctions(); + while (!QThread::currentThread()->isInterruptionRequested()) { if (!vipc_client || cur_stream_type != stream_type) { cur_stream_type = stream_type; @@ -287,10 +276,48 @@ void CameraViewWidget::vipcThread() { QThread::msleep(100); continue; } + + gl_buffer.reset(new QOpenGLBuffer(QOpenGLBuffer::PixelUnpackBuffer)); + gl_buffer->create(); + gl_buffer->bind(); + gl_buffer->setUsagePattern(QOpenGLBuffer::StreamDraw); + gl_buffer->allocate(vipc_client->buffers[0].len); + emit vipcThreadConnected(vipc_client.get()); } if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) { + { + std::lock_guard lk(lock); + + void *texture_buffer = gl_buffer->map(QOpenGLBuffer::WriteOnly); + + if (texture_buffer == nullptr) { + LOGE("gl_buffer->map returned nullptr"); + continue; + } + + memcpy(texture_buffer, buf->addr, buf->len); + gl_buffer->unmap(); + + // copy pixels from PBO to texture object + glBindTexture(GL_TEXTURE_2D, texture[buf->idx]->frame_tex); + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, buf->width, buf->height, GL_RGB, GL_UNSIGNED_BYTE, 0); + glBindTexture(GL_TEXTURE_2D, 0); + assert(glGetError() == GL_NO_ERROR); + + wait_fence.reset(new WaitFence()); + + // Ensure the fence is in the GPU command queue, or waiting on it might block + // https://www.khronos.org/opengl/wiki/Sync_Object#Flushing_and_contexts + glFlush(); + + latest_texture_id = buf->idx; + } + // Schedule update. update() will be invoked on the gui thread. + QMetaObject::invokeMethod(this, "update"); + + // TODO: remove later, it's only connected by DriverView. emit vipcThreadFrameReceived(buf); } } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 51114b2f22..03709cbdd8 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -8,6 +8,7 @@ #include #include "cereal/visionipc/visionipc_client.h" #include "selfdrive/camerad/cameras/camera_common.h" +#include "selfdrive/common/visionimg.h" #include "selfdrive/ui/ui.h" class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { @@ -44,9 +45,10 @@ protected: bool zoomed_view; std::mutex lock; - VisionBuf *latest_frame = nullptr; + int latest_texture_id = -1; GLuint frame_vao, frame_vbo, frame_ibo; mat4 frame_mat; + std::unique_ptr texture[UI_BUF_COUNT]; std::unique_ptr wait_fence; std::unique_ptr program; QColor bg = QColor("#000000"); @@ -57,8 +59,6 @@ protected: std::atomic stream_type; QThread *vipc_thread = nullptr; - GLuint textures[3]; - protected slots: void vipcConnected(VisionIpcClient *vipc_client); }; diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc index e09587023c..5027423ebc 100644 --- a/selfdrive/ui/replay/main.cc +++ b/selfdrive/ui/replay/main.cc @@ -15,6 +15,7 @@ int main(int argc, char *argv[]) { {"no-loop", REPLAY_FLAG_NO_LOOP, "stop at the end of the route"}, {"no-cache", REPLAY_FLAG_NO_FILE_CACHE, "turn off local cache"}, {"qcam", REPLAY_FLAG_QCAMERA, "load qcamera"}, + {"yuv", REPLAY_FLAG_SEND_YUV, "send yuv frame"}, {"no-hw-decoder", REPLAY_FLAG_NO_HW_DECODER, "disable HW video decoding"}, {"no-vipc", REPLAY_FLAG_NO_VIPC, "do not output video"}, }; diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index 1fdce6fd66..fd1a4b1990 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -289,7 +289,7 @@ void Replay::startStream(const Segment *cur_segment) { camera_size[type] = {fr->width, fr->height}; } } - camera_server_ = std::make_unique(camera_size, true); + camera_server_ = std::make_unique(camera_size, hasFlag(REPLAY_FLAG_SEND_YUV)); } // start stream thread diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h index c89a835f64..4b33c56267 100644 --- a/selfdrive/ui/replay/replay.h +++ b/selfdrive/ui/replay/replay.h @@ -17,6 +17,7 @@ enum REPLAY_FLAGS { REPLAY_FLAG_NO_LOOP = 0x0010, REPLAY_FLAG_NO_FILE_CACHE = 0x0020, REPLAY_FLAG_QCAMERA = 0x0040, + REPLAY_FLAG_SEND_YUV = 0x0080, REPLAY_FLAG_NO_HW_DECODER = 0x0100, REPLAY_FLAG_FULL_SPEED = 0x0200, REPLAY_FLAG_NO_VIPC = 0x0400, diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index c1d47d040d..74c00fe18e 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,16 +19,15 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - // TODO: make mapd output YUV - // hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false)); + hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_RGB_MAP, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_ROAD, false)); } { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_DRIVER, false)); - hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_DRIVER, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE_ROAD, false)); } return a.exec();