openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.

92 lines
2.9 KiB

#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("camerad", VISION_STREAM_DRIVER, true, this);
layout->addWidget(cameraView);
scene = new DriverViewScene(this);
connect(cameraView, &CameraViewWidget::vipcThreadFrameReceived, scene, &DriverViewScene::frameUpdated);
layout->addWidget(scene);
layout->setCurrentWidget(scene);
}
void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) {
emit done();
}
DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) {
face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE});
}
void DriverViewScene::showEvent(QShowEvent* event) {
frame_updated = false;
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(Qt::white);
p.setRenderHint(QPainter::TextAntialiasing);
configFont(p, "Inter", 100, "Bold");
p.drawText(geometry(), Qt::AlignCenter, tr("camera starting"));
return;
}
cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2();
cereal::DriverStateV2::DriverData::Reader driver_data;
is_rhd = driver_state.getWheelOnRightProb() > 0.5;
driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData();
bool face_detected = driver_data.getFaceProb() > 0.7;
if (face_detected) {
auto fxy_list = driver_data.getFacePosition();
auto std_list = driver_data.getFaceOrientationStd();
float face_x = fxy_list[0];
float face_y = fxy_list[1];
float face_std = std::max(std_list[0], std_list[1]);
float alpha = 0.7;
if (face_std > 0.15) {
alpha = std::max(0.7 - (face_std-0.15)*3.5, 0.0);
}
const int box_size = 220;
// use approx instead of distort_points
int fbox_x = 1080.0 - 1714.0 * face_x;
int fbox_y = -135.0 + (504.0 + std::abs(face_x)*112.0) + (1205.0 - std::abs(face_x)*724.0) * face_y;
p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10));
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 = 60;
const int img_x = is_rhd ? rect().right() - FACE_IMG_SIZE - img_offset : rect().left() + img_offset;
const int img_y = rect().bottom() - FACE_IMG_SIZE - img_offset;
p.setOpacity(face_detected ? 1.0 : 0.2);
p.drawPixmap(img_x, img_y, face_img);
}