Qt driverview (#21063)
* CameraViewWidget * continue * cleanup * mv DriverViewWindow to ui/qt/offroad * write IsDriverViewEnabled in showEvent/hideEvnet * sm.update(0) in onTimeout() * CameraViewWidget * use unique_ptr for vipc_client * virtual draw * fix viewport * connected()->frameReceived() * bg_colors use QColor * fix draw * rebase master * whitespace * apply reviews * indent * like onroad continue * white space * continue * show == false * remove border * use widget's size * fix shadowed rect * cleanup driverview * fix transform * remove video_rect Co-authored-by: deanlee <deanlee3@gmail.com> Co-authored-by: Comma Device <device@comma.ai>pull/21108/head
Adeeb Shihadeh
4 years ago
committed by
GitHub
parent
989ffa36e5
commit
9876723169
14 changed files with 503 additions and 176 deletions
@ -0,0 +1,109 @@ |
||||
#include "selfdrive/ui/qt/offroad/driverview.h" |
||||
|
||||
#include <QPainter> |
||||
|
||||
#include "selfdrive/ui/qt/qt_window.h" |
||||
#include "selfdrive/ui/qt/util.h" |
||||
|
||||
const int FACE_IMG_SIZE = 130; |
||||
|
||||
DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { |
||||
setAttribute(Qt::WA_OpaquePaintEvent); |
||||
layout = new QStackedLayout(this); |
||||
layout->setStackingMode(QStackedLayout::StackAll); |
||||
|
||||
cameraView = new CameraViewWidget(VISION_STREAM_RGB_FRONT, this); |
||||
layout->addWidget(cameraView); |
||||
|
||||
scene = new DriverViewScene(this); |
||||
connect(cameraView, &CameraViewWidget::frameUpdated, scene, &DriverViewScene::frameUpdated); |
||||
layout->addWidget(scene); |
||||
layout->setCurrentWidget(scene); |
||||
} |
||||
|
||||
void DriverViewWindow::mousePressEvent(QMouseEvent* e) { |
||||
emit done(); |
||||
} |
||||
|
||||
DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) { |
||||
face = QImage("../assets/img_driver_face.png").scaled(FACE_IMG_SIZE, FACE_IMG_SIZE, Qt::KeepAspectRatio, Qt::SmoothTransformation); |
||||
} |
||||
|
||||
void DriverViewScene::showEvent(QShowEvent* event) { |
||||
frame_updated = false; |
||||
is_rhd = params.getBool("IsRHD"); |
||||
params.putBool("IsDriverViewEnabled", true); |
||||
} |
||||
|
||||
void DriverViewScene::hideEvent(QHideEvent* event) { |
||||
params.putBool("IsDriverViewEnabled", false); |
||||
} |
||||
|
||||
void DriverViewScene::frameUpdated() { |
||||
frame_updated = true; |
||||
sm.update(0); |
||||
update(); |
||||
} |
||||
|
||||
void DriverViewScene::paintEvent(QPaintEvent* event) { |
||||
QPainter p(this); |
||||
|
||||
// startup msg
|
||||
if (!frame_updated) { |
||||
p.setPen(QColor(0xff, 0xff, 0xff)); |
||||
p.setRenderHint(QPainter::TextAntialiasing); |
||||
configFont(p, "Inter", 100, "Bold"); |
||||
p.drawText(geometry(), Qt::AlignCenter, "camera starting"); |
||||
return; |
||||
} |
||||
|
||||
const int width = 4 * height() / 3; |
||||
const QRect rect2 = {rect().center().x() - width / 2, rect().top(), width, rect().height()}; |
||||
const QRect valid_rect = {is_rhd ? rect2.right() - rect2.height() / 2 : rect2.left(), rect2.top(), rect2.height() / 2, rect2.height()}; |
||||
|
||||
// blackout
|
||||
const int blackout_x_r = valid_rect.right(); |
||||
const QRect& blackout_rect = Hardware::TICI() ? rect() : rect2; |
||||
const int blackout_w_r = blackout_rect.right() - valid_rect.right(); |
||||
const int blackout_x_l = blackout_rect.left(); |
||||
const int blackout_w_l = valid_rect.left() - blackout_x_l; |
||||
|
||||
QColor bg(0, 0, 0, 140); |
||||
p.setPen(QPen(bg)); |
||||
p.setBrush(QBrush(bg)); |
||||
p.drawRect(blackout_x_l, rect2.top(), blackout_w_l, rect2.height()); |
||||
p.drawRect(blackout_x_r, rect2.top(), blackout_w_r, rect2.height()); |
||||
|
||||
// face bounding box
|
||||
cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); |
||||
bool face_detected = driver_state.getFaceProb() > 0.4; |
||||
if (face_detected) { |
||||
auto fxy_list = driver_state.getFacePosition(); |
||||
float face_x = fxy_list[0]; |
||||
float face_y = fxy_list[1]; |
||||
int fbox_x = valid_rect.center().x() + (is_rhd ? face_x : -face_x) * valid_rect.width(); |
||||
int fbox_y = valid_rect.center().y() + face_y * valid_rect.height(); |
||||
|
||||
float alpha = 0.2; |
||||
face_x = std::abs(face_x); |
||||
face_y = std::abs(face_y); |
||||
if (face_x <= 0.35 && face_y <= 0.4) { |
||||
alpha = 0.8 - (face_x > face_y ? face_x : face_y) * 0.6 / 0.375; |
||||
} |
||||
|
||||
const int box_size = 0.6 * rect2.height() / 2; |
||||
QPen pen(QColor(255, 255, 255, alpha * 255)); |
||||
pen.setWidth(10); |
||||
p.setPen(pen); |
||||
p.setBrush(Qt::NoBrush); |
||||
p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0); |
||||
} |
||||
|
||||
// icon
|
||||
const int img_offset = 30; |
||||
const int img_x = is_rhd ? rect2.right() - FACE_IMG_SIZE - img_offset : rect2.left() + img_offset; |
||||
const int img_y = rect2.bottom() - FACE_IMG_SIZE - img_offset; |
||||
p.setPen(Qt::NoPen); |
||||
p.setOpacity(face_detected ? 1.0 : 0.3); |
||||
p.drawImage(img_x, img_y, face); |
||||
} |
||||
@ -0,0 +1,48 @@ |
||||
#pragma once |
||||
|
||||
#include <memory> |
||||
|
||||
#include <QStackedLayout> |
||||
|
||||
#include "selfdrive/common/util.h" |
||||
#include "selfdrive/ui/qt/widgets/cameraview.h" |
||||
|
||||
class DriverViewScene : public QWidget { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
explicit DriverViewScene(QWidget *parent); |
||||
|
||||
public slots: |
||||
void frameUpdated(); |
||||
|
||||
protected: |
||||
void showEvent(QShowEvent *event) override; |
||||
void hideEvent(QHideEvent *event) override; |
||||
void paintEvent(QPaintEvent *event) override; |
||||
|
||||
private: |
||||
Params params; |
||||
SubMaster sm; |
||||
QImage face; |
||||
bool is_rhd = false; |
||||
bool frame_updated = false; |
||||
}; |
||||
|
||||
class DriverViewWindow : public QWidget { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
explicit DriverViewWindow(QWidget *parent); |
||||
|
||||
signals: |
||||
void done(); |
||||
|
||||
protected: |
||||
void mousePressEvent(QMouseEvent* e) override; |
||||
|
||||
private: |
||||
CameraViewWidget *cameraView; |
||||
DriverViewScene *scene; |
||||
QStackedLayout *layout; |
||||
}; |
||||
@ -0,0 +1,217 @@ |
||||
#include "selfdrive/ui/qt/widgets/cameraview.h" |
||||
|
||||
#include "selfdrive/ui/qt/qt_window.h" |
||||
|
||||
namespace { |
||||
const char frame_vertex_shader[] = |
||||
#ifdef NANOVG_GL3_IMPLEMENTATION |
||||
"#version 150 core\n" |
||||
#else |
||||
"#version 300 es\n" |
||||
#endif |
||||
"in vec4 aPosition;\n" |
||||
"in vec4 aTexCoord;\n" |
||||
"uniform mat4 uTransform;\n" |
||||
"out vec4 vTexCoord;\n" |
||||
"void main() {\n" |
||||
" gl_Position = uTransform * aPosition;\n" |
||||
" vTexCoord = aTexCoord;\n" |
||||
"}\n"; |
||||
|
||||
const char frame_fragment_shader[] = |
||||
#ifdef NANOVG_GL3_IMPLEMENTATION |
||||
"#version 150 core\n" |
||||
#else |
||||
"#version 300 es\n" |
||||
#endif |
||||
"precision mediump float;\n" |
||||
"uniform sampler2D uTexture;\n" |
||||
"in vec4 vTexCoord;\n" |
||||
"out vec4 colorOut;\n" |
||||
"void main() {\n" |
||||
" colorOut = texture(uTexture, vTexCoord.xy);\n" |
||||
#ifdef QCOM |
||||
" vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n" |
||||
" colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n" |
||||
#endif |
||||
"}\n"; |
||||
|
||||
const mat4 device_transform = {{ |
||||
1.0, 0.0, 0.0, 0.0, |
||||
0.0, 1.0, 0.0, 0.0, |
||||
0.0, 0.0, 1.0, 0.0, |
||||
0.0, 0.0, 0.0, 1.0, |
||||
}}; |
||||
|
||||
mat4 get_driver_view_transform() { |
||||
const float driver_view_ratio = 1.333; |
||||
mat4 transform; |
||||
if (Hardware::TICI()) { |
||||
// from dmonitoring.cc
|
||||
const int full_width_tici = 1928; |
||||
const int full_height_tici = 1208; |
||||
const int adapt_width_tici = 668; |
||||
const int crop_x_offset = 32; |
||||
const int crop_y_offset = -196; |
||||
const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici; |
||||
const float xscale = yscale*(1080)/(2160)*full_width_tici/full_height_tici; |
||||
transform = (mat4){{ |
||||
xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2, |
||||
0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2, |
||||
0.0, 0.0, 1.0, 0.0, |
||||
0.0, 0.0, 0.0, 1.0, |
||||
}}; |
||||
} else { |
||||
// frame from 4/3 to 16/9 display
|
||||
transform = (mat4){{ |
||||
driver_view_ratio*(1080)/(1920), 0.0, 0.0, 0.0, |
||||
0.0, 1.0, 0.0, 0.0, |
||||
0.0, 0.0, 1.0, 0.0, |
||||
0.0, 0.0, 0.0, 1.0, |
||||
}}; |
||||
} |
||||
return transform; |
||||
} |
||||
|
||||
} // namespace
|
||||
|
||||
CameraViewWidget::CameraViewWidget(VisionStreamType stream_type, QWidget* parent) : stream_type(stream_type), QOpenGLWidget(parent) { |
||||
setAttribute(Qt::WA_OpaquePaintEvent); |
||||
|
||||
timer = new QTimer(this); |
||||
connect(timer, &QTimer::timeout, this, &CameraViewWidget::updateFrame); |
||||
} |
||||
|
||||
CameraViewWidget::~CameraViewWidget() { |
||||
makeCurrent(); |
||||
doneCurrent(); |
||||
glDeleteVertexArrays(1, &frame_vao); |
||||
glDeleteBuffers(1, &frame_vbo); |
||||
glDeleteBuffers(1, &frame_ibo); |
||||
} |
||||
|
||||
void CameraViewWidget::initializeGL() { |
||||
initializeOpenGLFunctions(); |
||||
|
||||
gl_shader = std::make_unique<GLShader>(frame_vertex_shader, frame_fragment_shader); |
||||
GLint frame_pos_loc = glGetAttribLocation(gl_shader->prog, "aPosition"); |
||||
GLint frame_texcoord_loc = glGetAttribLocation(gl_shader->prog, "aTexCoord"); |
||||
|
||||
auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? 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
|
||||
{-1.0, 1.0, x2, y2}, //tl
|
||||
{ 1.0, 1.0, x1, y2}, //tr
|
||||
{ 1.0, -1.0, x1, y1}, //br
|
||||
}; |
||||
|
||||
glGenVertexArrays(1, &frame_vao); |
||||
glBindVertexArray(frame_vao); |
||||
glGenBuffers(1, &frame_vbo); |
||||
glBindBuffer(GL_ARRAY_BUFFER, frame_vbo); |
||||
glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); |
||||
glEnableVertexAttribArray(frame_pos_loc); |
||||
glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, |
||||
sizeof(frame_coords[0]), (const void *)0); |
||||
glEnableVertexAttribArray(frame_texcoord_loc); |
||||
glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, |
||||
sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); |
||||
glGenBuffers(1, &frame_ibo); |
||||
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, frame_ibo); |
||||
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); |
||||
glBindBuffer(GL_ARRAY_BUFFER, 0); |
||||
glBindVertexArray(0); |
||||
|
||||
if (stream_type == VISION_STREAM_RGB_FRONT) { |
||||
frame_mat = matmul(device_transform, get_driver_view_transform()); |
||||
} else { |
||||
auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; |
||||
float zoom_ = zoom / intrinsic_matrix.v[0]; |
||||
if (stream_type == VISION_STREAM_RGB_WIDE) { |
||||
zoom_ *= 0.5; |
||||
} |
||||
float zx = zoom_ * 2 * intrinsic_matrix.v[2] / width(); |
||||
float zy = zoom_ * 2 * intrinsic_matrix.v[5] / height(); |
||||
|
||||
const mat4 frame_transform = {{ |
||||
zx, 0.0, 0.0, 0.0, |
||||
0.0, zy, 0.0, -y_offset / height() * 2, |
||||
0.0, 0.0, 1.0, 0.0, |
||||
0.0, 0.0, 0.0, 1.0, |
||||
}}; |
||||
frame_mat = matmul(device_transform, frame_transform); |
||||
} |
||||
vipc_client = std::make_unique<VisionIpcClient>("camerad", stream_type, true); |
||||
} |
||||
|
||||
void CameraViewWidget::showEvent(QShowEvent *event) { |
||||
timer->start(0); |
||||
} |
||||
|
||||
void CameraViewWidget::hideEvent(QHideEvent *event) { |
||||
timer->stop(); |
||||
vipc_client->connected = false; |
||||
latest_frame = nullptr; |
||||
} |
||||
|
||||
void CameraViewWidget::paintGL() { |
||||
if (!latest_frame) { |
||||
glClearColor(0, 0, 0, 1.0); |
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); |
||||
return; |
||||
} |
||||
|
||||
glViewport(0, 0, width(), height()); |
||||
|
||||
glBindVertexArray(frame_vao); |
||||
glActiveTexture(GL_TEXTURE0); |
||||
|
||||
glBindTexture(GL_TEXTURE_2D, texture[latest_frame->idx]->frame_tex); |
||||
if (!Hardware::EON()) { |
||||
// this is handled in ion on QCOM
|
||||
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, latest_frame->width, latest_frame->height, |
||||
0, GL_RGB, GL_UNSIGNED_BYTE, latest_frame->addr); |
||||
} |
||||
|
||||
glUseProgram(gl_shader->prog); |
||||
glUniform1i(gl_shader->getUniformLocation("uTexture"), 0); |
||||
glUniformMatrix4fv(gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); |
||||
|
||||
assert(glGetError() == GL_NO_ERROR); |
||||
glEnableVertexAttribArray(0); |
||||
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); |
||||
glDisableVertexAttribArray(0); |
||||
glBindVertexArray(0); |
||||
} |
||||
|
||||
void CameraViewWidget::updateFrame() { |
||||
if (!vipc_client->connected && vipc_client->connect(false)) { |
||||
// init vision
|
||||
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); |
||||
|
||||
// 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_frame = nullptr; |
||||
} |
||||
|
||||
if (vipc_client->connected) { |
||||
VisionBuf *buf = vipc_client->recv(); |
||||
if (buf != nullptr) { |
||||
latest_frame = buf; |
||||
update(); |
||||
emit frameUpdated(); |
||||
} else { |
||||
LOGE("visionIPC receive timeout"); |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,44 @@ |
||||
#pragma once |
||||
|
||||
#include <memory> |
||||
|
||||
#include <QOpenGLFunctions> |
||||
#include <QOpenGLWidget> |
||||
|
||||
#include "cereal/visionipc/visionipc_client.h" |
||||
#include "selfdrive/common/glutil.h" |
||||
#include "selfdrive/common/mat.h" |
||||
#include "selfdrive/common/visionimg.h" |
||||
#include "selfdrive/ui/ui.h" |
||||
|
||||
class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
using QOpenGLWidget::QOpenGLWidget; |
||||
explicit CameraViewWidget(VisionStreamType stream_type, QWidget* parent = nullptr); |
||||
~CameraViewWidget(); |
||||
|
||||
signals: |
||||
void frameUpdated(); |
||||
|
||||
protected: |
||||
void paintGL() override; |
||||
void initializeGL() override; |
||||
void showEvent(QShowEvent *event) override; |
||||
void hideEvent(QHideEvent *event) override; |
||||
|
||||
protected slots: |
||||
void updateFrame(); |
||||
|
||||
private: |
||||
VisionBuf *latest_frame = nullptr; |
||||
GLuint frame_vao, frame_vbo, frame_ibo; |
||||
mat4 frame_mat; |
||||
std::unique_ptr<VisionIpcClient> vipc_client; |
||||
std::unique_ptr<EGLImageTexture> texture[UI_BUF_COUNT]; |
||||
std::unique_ptr<GLShader> gl_shader; |
||||
|
||||
VisionStreamType stream_type; |
||||
QTimer* timer; |
||||
}; |
||||
Loading…
Reference in new issue