|
|
|
|
@ -97,37 +97,22 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) |
|
|
|
|
|
|
|
|
|
} // namespace
|
|
|
|
|
|
|
|
|
|
CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : |
|
|
|
|
stream_name(stream_name), active_stream_type(type), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { |
|
|
|
|
setAttribute(Qt::WA_OpaquePaintEvent); |
|
|
|
|
qRegisterMetaType<std::set<VisionStreamType>>("availableStreams"); |
|
|
|
|
QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); |
|
|
|
|
QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); |
|
|
|
|
QObject::connect(this, &CameraWidget::vipcAvailableStreamsUpdated, this, &CameraWidget::availableStreamsUpdated, Qt::QueuedConnection); |
|
|
|
|
CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget *parent) |
|
|
|
|
: stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
CameraWidget::~CameraWidget() { |
|
|
|
|
makeCurrent(); |
|
|
|
|
stopVipcThread(); |
|
|
|
|
if (isValid()) { |
|
|
|
|
glDeleteVertexArrays(1, &frame_vao); |
|
|
|
|
glDeleteBuffers(1, &frame_vbo); |
|
|
|
|
glDeleteBuffers(1, &frame_ibo); |
|
|
|
|
glDeleteBuffers(2, textures); |
|
|
|
|
} |
|
|
|
|
clearEGLImages(); |
|
|
|
|
doneCurrent(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Qt uses device-independent pixels, depending on platform this may be
|
|
|
|
|
// different to what OpenGL uses
|
|
|
|
|
int CameraWidget::glWidth() { |
|
|
|
|
return width() * devicePixelRatio(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int CameraWidget::glHeight() { |
|
|
|
|
return height() * devicePixelRatio(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::initializeGL() { |
|
|
|
|
initializeOpenGLFunctions(); |
|
|
|
|
|
|
|
|
|
@ -141,7 +126,7 @@ void CameraWidget::initializeGL() { |
|
|
|
|
GLint frame_pos_loc = program->attributeLocation("aPosition"); |
|
|
|
|
GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); |
|
|
|
|
|
|
|
|
|
auto [x1, x2, y1, y2] = requested_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_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
|
|
|
|
|
@ -178,45 +163,11 @@ void CameraWidget::initializeGL() { |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::showEvent(QShowEvent *event) { |
|
|
|
|
if (!vipc_thread) { |
|
|
|
|
clearFrames(); |
|
|
|
|
vipc_thread = new QThread(); |
|
|
|
|
connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); |
|
|
|
|
connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater); |
|
|
|
|
vipc_thread->start(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::stopVipcThread() { |
|
|
|
|
makeCurrent(); |
|
|
|
|
if (vipc_thread) { |
|
|
|
|
vipc_thread->requestInterruption(); |
|
|
|
|
vipc_thread->quit(); |
|
|
|
|
vipc_thread->wait(); |
|
|
|
|
vipc_thread = nullptr; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#ifdef QCOM2 |
|
|
|
|
EGLDisplay egl_display = eglGetCurrentDisplay(); |
|
|
|
|
assert(egl_display != EGL_NO_DISPLAY); |
|
|
|
|
for (auto &pair : egl_images) { |
|
|
|
|
eglDestroyImageKHR(egl_display, pair.second); |
|
|
|
|
assert(eglGetError() == EGL_SUCCESS); |
|
|
|
|
} |
|
|
|
|
egl_images.clear(); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::availableStreamsUpdated(std::set<VisionStreamType> streams) { |
|
|
|
|
available_streams = streams; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::updateFrameMat() { |
|
|
|
|
int w = glWidth(), h = glHeight(); |
|
|
|
|
|
|
|
|
|
if (zoomed_view) { |
|
|
|
|
if (active_stream_type == VISION_STREAM_DRIVER) { |
|
|
|
|
if (streamType() == VISION_STREAM_DRIVER) { |
|
|
|
|
if (stream_width > 0 && stream_height > 0) { |
|
|
|
|
frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); |
|
|
|
|
} |
|
|
|
|
@ -225,7 +176,7 @@ void CameraWidget::updateFrameMat() { |
|
|
|
|
// to ensure this ends up in the middle of the screen
|
|
|
|
|
// for narrow come and a little lower for wide cam.
|
|
|
|
|
// TODO: use proper perspective transform?
|
|
|
|
|
if (active_stream_type == VISION_STREAM_WIDE_ROAD) { |
|
|
|
|
if (streamType() == VISION_STREAM_WIDE_ROAD) { |
|
|
|
|
intrinsic_matrix = ECAM_INTRINSIC_MATRIX; |
|
|
|
|
zoom = 2.0; |
|
|
|
|
} else { |
|
|
|
|
@ -247,13 +198,12 @@ void CameraWidget::updateFrameMat() { |
|
|
|
|
|
|
|
|
|
float zx = zoom * 2 * intrinsic_matrix.v[2] / w; |
|
|
|
|
float zy = zoom * 2 * intrinsic_matrix.v[5] / h; |
|
|
|
|
const mat4 frame_transform = {{ |
|
|
|
|
frame_mat = {{ |
|
|
|
|
zx, 0.0, 0.0, -x_offset / w * 2, |
|
|
|
|
0.0, zy, 0.0, y_offset / h * 2, |
|
|
|
|
0.0, 0.0, 1.0, 0.0, |
|
|
|
|
0.0, 0.0, 0.0, 1.0, |
|
|
|
|
}}; |
|
|
|
|
frame_mat = frame_transform; |
|
|
|
|
} |
|
|
|
|
} else if (stream_width > 0 && stream_height > 0) { |
|
|
|
|
// fit frame to widget size
|
|
|
|
|
@ -271,25 +221,15 @@ void CameraWidget::paintGL() { |
|
|
|
|
glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); |
|
|
|
|
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); |
|
|
|
|
|
|
|
|
|
std::lock_guard lk(frame_lock); |
|
|
|
|
if (frames.empty()) return; |
|
|
|
|
|
|
|
|
|
int frame_idx = frames.size() - 1; |
|
|
|
|
|
|
|
|
|
// Always draw latest frame until sync logic is more stable
|
|
|
|
|
// for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) {
|
|
|
|
|
// if (frames[frame_idx].first == draw_frame_id) break;
|
|
|
|
|
// }
|
|
|
|
|
VisionBuf *frame = receiveFrame(draw_frame_id); |
|
|
|
|
if (!frame) return; |
|
|
|
|
|
|
|
|
|
// Log duplicate/dropped frames
|
|
|
|
|
if (frames[frame_idx].first == prev_frame_id) { |
|
|
|
|
qDebug() << "Drawing same frame twice" << frames[frame_idx].first; |
|
|
|
|
} else if (frames[frame_idx].first != prev_frame_id + 1) { |
|
|
|
|
qDebug() << "Skipped frame" << frames[frame_idx].first; |
|
|
|
|
uint32_t frame_id = frame->get_frame_id(); |
|
|
|
|
if (prev_frame_id != INVALID_FRAME_ID && frame_id != prev_frame_id + 1) { |
|
|
|
|
qDebug() << (frame_id == prev_frame_id ? "Drawing same frame twice" : "Skip frame") << frame_id; |
|
|
|
|
} |
|
|
|
|
prev_frame_id = frames[frame_idx].first; |
|
|
|
|
VisionBuf *frame = frames[frame_idx].second; |
|
|
|
|
assert(frame != nullptr); |
|
|
|
|
prev_frame_id = frame_id; |
|
|
|
|
|
|
|
|
|
updateFrameMat(); |
|
|
|
|
|
|
|
|
|
@ -329,20 +269,14 @@ void CameraWidget::paintGL() { |
|
|
|
|
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { |
|
|
|
|
makeCurrent(); |
|
|
|
|
void CameraWidget::vipcConnected() { |
|
|
|
|
stream_width = vipc_client->buffers[0].width; |
|
|
|
|
stream_height = vipc_client->buffers[0].height; |
|
|
|
|
stream_stride = vipc_client->buffers[0].stride; |
|
|
|
|
|
|
|
|
|
#ifdef QCOM2 |
|
|
|
|
clearEGLImages(); |
|
|
|
|
EGLDisplay egl_display = eglGetCurrentDisplay(); |
|
|
|
|
assert(egl_display != EGL_NO_DISPLAY); |
|
|
|
|
for (auto &pair : egl_images) { |
|
|
|
|
eglDestroyImageKHR(egl_display, pair.second); |
|
|
|
|
} |
|
|
|
|
egl_images.clear(); |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < vipc_client->num_buffers; i++) { // import buffers into OpenGL
|
|
|
|
|
int fd = dup(vipc_client->buffers[i].fd); // eglDestroyImageKHR will close, so duplicate
|
|
|
|
|
EGLint img_attrs[] = { |
|
|
|
|
@ -379,59 +313,78 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::vipcFrameReceived() { |
|
|
|
|
update(); |
|
|
|
|
VisionBuf *CameraWidget::receiveFrame(uint64_t request_frame_id) { |
|
|
|
|
if (!ensureConnection()) { |
|
|
|
|
return nullptr; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Receive frames and store them in recent_frames
|
|
|
|
|
while (auto buf = vipc_client->recv(nullptr, 0)) { |
|
|
|
|
recent_frames.emplace_back(buf); |
|
|
|
|
if (recent_frames.size() > FRAME_BUFFER_SIZE) { |
|
|
|
|
recent_frames.pop_front(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (!vipc_client->connected || recent_frames.empty()) { |
|
|
|
|
return nullptr; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::vipcThread() { |
|
|
|
|
VisionStreamType cur_stream = requested_stream_type; |
|
|
|
|
std::unique_ptr<VisionIpcClient> vipc_client; |
|
|
|
|
VisionIpcBufExtra meta_main = {0}; |
|
|
|
|
// Find the requested frame
|
|
|
|
|
auto it = std::find_if(recent_frames.rbegin(), recent_frames.rend(), |
|
|
|
|
[request_frame_id](VisionBuf *buf) { return buf->get_frame_id() == request_frame_id; }); |
|
|
|
|
return it != recent_frames.rend() ? *it : recent_frames.back(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
while (!QThread::currentThread()->isInterruptionRequested()) { |
|
|
|
|
if (!vipc_client || cur_stream != requested_stream_type) { |
|
|
|
|
clearFrames(); |
|
|
|
|
qDebug().nospace() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; |
|
|
|
|
cur_stream = requested_stream_type; |
|
|
|
|
vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); |
|
|
|
|
bool CameraWidget::ensureConnection() { |
|
|
|
|
// Reconnect if the client is not initialized or the stream type has changed
|
|
|
|
|
if (!vipc_client || vipc_client->type != stream_type) { |
|
|
|
|
qDebug() << "connecting to stream" << stream_type; |
|
|
|
|
vipc_client.reset(new VisionIpcClient(stream_name, stream_type, false)); |
|
|
|
|
} |
|
|
|
|
active_stream_type = cur_stream; |
|
|
|
|
|
|
|
|
|
// Re-establish connection if not connected
|
|
|
|
|
if (!vipc_client->connected) { |
|
|
|
|
clearFrames(); |
|
|
|
|
auto streams = VisionIpcClient::getAvailableStreams(stream_name, false); |
|
|
|
|
if (streams.empty()) { |
|
|
|
|
QThread::msleep(100); |
|
|
|
|
continue; |
|
|
|
|
available_streams = VisionIpcClient::getAvailableStreams(stream_name, false); |
|
|
|
|
if (available_streams.empty() || !vipc_client->connect(false)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
emit vipcAvailableStreamsUpdated(streams); |
|
|
|
|
|
|
|
|
|
if (!vipc_client->connect(false)) { |
|
|
|
|
QThread::msleep(100); |
|
|
|
|
continue; |
|
|
|
|
emit vipcAvailableStreamsUpdated(); |
|
|
|
|
vipcConnected(); |
|
|
|
|
} |
|
|
|
|
emit vipcThreadConnected(vipc_client.get()); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) { |
|
|
|
|
{ |
|
|
|
|
std::lock_guard lk(frame_lock); |
|
|
|
|
frames.push_back(std::make_pair(meta_main.frame_id, buf)); |
|
|
|
|
while (frames.size() > FRAME_BUFFER_SIZE) { |
|
|
|
|
frames.pop_front(); |
|
|
|
|
} |
|
|
|
|
void CameraWidget::clearFrames() { |
|
|
|
|
recent_frames.clear(); |
|
|
|
|
draw_frame_id = INVALID_FRAME_ID; |
|
|
|
|
prev_frame_id = INVALID_FRAME_ID; |
|
|
|
|
} |
|
|
|
|
emit vipcThreadFrameReceived(); |
|
|
|
|
} else { |
|
|
|
|
if (!isVisible()) { |
|
|
|
|
vipc_client->connected = false; |
|
|
|
|
|
|
|
|
|
void CameraWidget::clearEGLImages() { |
|
|
|
|
#ifdef QCOM2 |
|
|
|
|
EGLDisplay egl_display = eglGetCurrentDisplay(); |
|
|
|
|
assert(egl_display != EGL_NO_DISPLAY); |
|
|
|
|
for (auto &pair : egl_images) { |
|
|
|
|
eglDestroyImageKHR(egl_display, pair.second); |
|
|
|
|
} |
|
|
|
|
egl_images.clear(); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Cameraview
|
|
|
|
|
|
|
|
|
|
CameraView::CameraView(const std::string &name, VisionStreamType stream_type, bool zoom, QWidget *parent) |
|
|
|
|
: CameraWidget(name, stream_type, zoom, parent) { |
|
|
|
|
timer = new QTimer(this); |
|
|
|
|
timer->setInterval(1000.0 / UI_FREQ); |
|
|
|
|
timer->callOnTimeout(this, [this]() { update(); }); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraView::showEvent(QShowEvent *event) { |
|
|
|
|
timer->start(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void CameraWidget::clearFrames() { |
|
|
|
|
std::lock_guard lk(frame_lock); |
|
|
|
|
frames.clear(); |
|
|
|
|
available_streams.clear(); |
|
|
|
|
void CameraView::hideEvent(QHideEvent *event) { |
|
|
|
|
timer->stop(); |
|
|
|
|
} |
|
|
|
|
|
Dean Lee
1 year ago
committed by
GitHub