b11db103c8
Added toggles to customize the lane lines, path, road edges, path edges, and an "Unlimited Length" mode that extends the road UI out as far as the model can see.
1019 lines
39 KiB
C++
1019 lines
39 KiB
C++
#include "selfdrive/ui/qt/onroad.h"
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <map>
|
||
#include <memory>
|
||
#include <sstream>
|
||
|
||
#include <QDebug>
|
||
#include <QMouseEvent>
|
||
|
||
#include "common/swaglog.h"
|
||
#include "common/timing.h"
|
||
#include "selfdrive/ui/qt/util.h"
|
||
#ifdef ENABLE_MAPS
|
||
#include "selfdrive/ui/qt/maps/map_helpers.h"
|
||
#include "selfdrive/ui/qt/maps/map_panel.h"
|
||
#endif
|
||
|
||
static void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity) {
|
||
p.setRenderHint(QPainter::Antialiasing);
|
||
p.setOpacity(1.0); // bg dictates opacity of ellipse
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(bg);
|
||
p.drawEllipse(center, btn_size / 2, btn_size / 2);
|
||
p.setOpacity(opacity);
|
||
p.drawPixmap(center - QPoint(img.width() / 2, img.height() / 2), img);
|
||
p.setOpacity(1.0);
|
||
}
|
||
|
||
OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->scene) {
|
||
QVBoxLayout *main_layout = new QVBoxLayout(this);
|
||
main_layout->setMargin(UI_BORDER_SIZE);
|
||
QStackedLayout *stacked_layout = new QStackedLayout;
|
||
stacked_layout->setStackingMode(QStackedLayout::StackAll);
|
||
main_layout->addLayout(stacked_layout);
|
||
|
||
nvg = new AnnotatedCameraWidget(VISION_STREAM_ROAD, this);
|
||
|
||
QWidget * split_wrapper = new QWidget;
|
||
split = new QHBoxLayout(split_wrapper);
|
||
split->setContentsMargins(0, 0, 0, 0);
|
||
split->setSpacing(0);
|
||
split->addWidget(nvg);
|
||
|
||
if (getenv("DUAL_CAMERA_VIEW")) {
|
||
CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this);
|
||
split->insertWidget(0, arCam);
|
||
}
|
||
|
||
if (getenv("MAP_RENDER_VIEW")) {
|
||
CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this);
|
||
split->insertWidget(0, map_render);
|
||
}
|
||
|
||
stacked_layout->addWidget(split_wrapper);
|
||
|
||
alerts = new OnroadAlerts(this);
|
||
alerts->setAttribute(Qt::WA_TransparentForMouseEvents, true);
|
||
stacked_layout->addWidget(alerts);
|
||
|
||
// setup stacking order
|
||
alerts->raise();
|
||
|
||
setAttribute(Qt::WA_OpaquePaintEvent);
|
||
QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState);
|
||
QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition);
|
||
QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged);
|
||
}
|
||
|
||
void OnroadWindow::updateState(const UIState &s) {
|
||
if (!s.scene.started) {
|
||
return;
|
||
}
|
||
|
||
QColor bgColor = bg_colors[s.status];
|
||
Alert alert = Alert::get(*(s.sm), s.scene.started_frame);
|
||
alerts->updateAlert(alert);
|
||
|
||
if (s.scene.map_on_left) {
|
||
split->setDirection(QBoxLayout::LeftToRight);
|
||
} else {
|
||
split->setDirection(QBoxLayout::RightToLeft);
|
||
}
|
||
|
||
nvg->updateState(s);
|
||
|
||
if (bg != bgColor) {
|
||
// repaint border
|
||
bg = bgColor;
|
||
update();
|
||
}
|
||
}
|
||
|
||
void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
||
Params params = Params();
|
||
|
||
// FrogPilot clickable widgets
|
||
bool widgetClicked = false;
|
||
|
||
#ifdef ENABLE_MAPS
|
||
if (map != nullptr && !widgetClicked) {
|
||
// Switch between map and sidebar when using navigate on openpilot
|
||
bool sidebarVisible = geometry().x() > 0;
|
||
bool show_map = uiState()->scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible;
|
||
map->setVisible(show_map && !map->isVisible());
|
||
}
|
||
#endif
|
||
// propagation event to parent(HomeWindow)
|
||
if (!widgetClicked) {
|
||
QWidget::mousePressEvent(e);
|
||
}
|
||
}
|
||
|
||
void OnroadWindow::offroadTransition(bool offroad) {
|
||
#ifdef ENABLE_MAPS
|
||
if (!offroad) {
|
||
if (map == nullptr && (uiState()->hasPrime() || !MAPBOX_TOKEN.isEmpty())) {
|
||
auto m = new MapPanel(get_mapbox_settings());
|
||
map = m;
|
||
|
||
QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested);
|
||
QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings);
|
||
QObject::connect(nvg->map_settings_btn_bottom, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings);
|
||
nvg->map_settings_btn->setEnabled(true);
|
||
|
||
m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE);
|
||
split->insertWidget(0, m);
|
||
|
||
// hidden by default, made visible when navRoute is published
|
||
m->setVisible(false);
|
||
}
|
||
}
|
||
#endif
|
||
|
||
alerts->updateAlert({});
|
||
}
|
||
|
||
void OnroadWindow::primeChanged(bool prime) {
|
||
#ifdef ENABLE_MAPS
|
||
if (map && (!prime && MAPBOX_TOKEN.isEmpty())) {
|
||
nvg->map_settings_btn->setEnabled(false);
|
||
nvg->map_settings_btn->setVisible(false);
|
||
map->deleteLater();
|
||
map = nullptr;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void OnroadWindow::paintEvent(QPaintEvent *event) {
|
||
QPainter p(this);
|
||
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
|
||
}
|
||
|
||
// ***** onroad widgets *****
|
||
|
||
// OnroadAlerts
|
||
void OnroadAlerts::updateAlert(const Alert &a) {
|
||
if (!alert.equal(a)) {
|
||
alert = a;
|
||
update();
|
||
}
|
||
}
|
||
|
||
void OnroadAlerts::paintEvent(QPaintEvent *event) {
|
||
if (alert.size == cereal::ControlsState::AlertSize::NONE) {
|
||
return;
|
||
}
|
||
static std::map<cereal::ControlsState::AlertSize, const int> alert_heights = {
|
||
{cereal::ControlsState::AlertSize::SMALL, 271},
|
||
{cereal::ControlsState::AlertSize::MID, 420},
|
||
{cereal::ControlsState::AlertSize::FULL, height()},
|
||
};
|
||
int h = alert_heights[alert.size];
|
||
|
||
int margin = 40;
|
||
int radius = 30;
|
||
int offset = scene.always_on_lateral || scene.conditional_experimental ? 25 : 0;
|
||
if (alert.size == cereal::ControlsState::AlertSize::FULL) {
|
||
margin = 0;
|
||
radius = 0;
|
||
offset = 0;
|
||
}
|
||
QRect r = QRect(0 + margin, height() - h + margin - offset, width() - margin*2, h - margin*2);
|
||
|
||
QPainter p(this);
|
||
|
||
// draw background + gradient
|
||
p.setPen(Qt::NoPen);
|
||
p.setCompositionMode(QPainter::CompositionMode_SourceOver);
|
||
p.setBrush(QBrush(alert_colors[alert.status]));
|
||
p.drawRoundedRect(r, radius, radius);
|
||
|
||
QLinearGradient g(0, r.y(), 0, r.bottom());
|
||
g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05));
|
||
g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35));
|
||
|
||
p.setCompositionMode(QPainter::CompositionMode_DestinationOver);
|
||
p.setBrush(QBrush(g));
|
||
p.drawRoundedRect(r, radius, radius);
|
||
p.setCompositionMode(QPainter::CompositionMode_SourceOver);
|
||
|
||
// text
|
||
const QPoint c = r.center();
|
||
p.setPen(QColor(0xff, 0xff, 0xff));
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
if (alert.size == cereal::ControlsState::AlertSize::SMALL) {
|
||
p.setFont(InterFont(74, QFont::DemiBold));
|
||
p.drawText(r, Qt::AlignCenter, alert.text1);
|
||
} else if (alert.size == cereal::ControlsState::AlertSize::MID) {
|
||
p.setFont(InterFont(88, QFont::Bold));
|
||
p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1);
|
||
p.setFont(InterFont(66));
|
||
p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2);
|
||
} else if (alert.size == cereal::ControlsState::AlertSize::FULL) {
|
||
bool l = alert.text1.length() > 15;
|
||
p.setFont(InterFont(l ? 132 : 177, QFont::Bold));
|
||
p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1);
|
||
p.setFont(InterFont(88));
|
||
p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2);
|
||
}
|
||
}
|
||
|
||
// ExperimentalButton
|
||
ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) {
|
||
setFixedSize(btn_size, btn_size);
|
||
|
||
engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
|
||
experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
|
||
QObject::connect(this, &QPushButton::clicked, this, &ExperimentalButton::changeMode);
|
||
}
|
||
|
||
void ExperimentalButton::changeMode() {
|
||
Params paramsMemory = Params("/dev/shm/params");
|
||
|
||
const auto cp = (*uiState()->sm)["carParams"].getCarParams();
|
||
bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
|
||
if (can_change) {
|
||
params.putBool("ExperimentalMode", !experimental_mode);
|
||
}
|
||
}
|
||
|
||
void ExperimentalButton::updateState(const UIState &s) {
|
||
const auto cs = (*s.sm)["controlsState"].getControlsState();
|
||
bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active;
|
||
if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
|
||
engageable = eng;
|
||
experimental_mode = cs.getExperimentalMode();
|
||
update();
|
||
}
|
||
|
||
// FrogPilot variables
|
||
}
|
||
|
||
void ExperimentalButton::paintEvent(QPaintEvent *event) {
|
||
QPainter p(this);
|
||
QPixmap img = experimental_mode ? experimental_img : engage_img;
|
||
drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0);
|
||
}
|
||
|
||
|
||
// MapSettingsButton
|
||
MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) {
|
||
setFixedSize(btn_size, btn_size + 20);
|
||
settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size});
|
||
|
||
// hidden by default, made visible if map is created (has prime or mapbox token)
|
||
setVisible(false);
|
||
setEnabled(false);
|
||
}
|
||
|
||
void MapSettingsButton::paintEvent(QPaintEvent *event) {
|
||
QPainter p(this);
|
||
drawIcon(p, QPoint(btn_size / 2, btn_size / 2), settings_img, QColor(0, 0, 0, 166), isDown() ? 0.6 : 1.0);
|
||
}
|
||
|
||
|
||
// Window that shows camera view and variety of info drawn on top
|
||
AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent), scene(uiState()->scene) {
|
||
pm = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"uiDebug"});
|
||
|
||
main_layout = new QVBoxLayout(this);
|
||
main_layout->setMargin(UI_BORDER_SIZE);
|
||
main_layout->setSpacing(0);
|
||
|
||
experimental_btn = new ExperimentalButton(this);
|
||
main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
|
||
|
||
map_settings_btn = new MapSettingsButton(this);
|
||
main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight);
|
||
|
||
dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
|
||
|
||
// Initialize FrogPilot widgets
|
||
initializeFrogPilotWidgets();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateState(const UIState &s) {
|
||
const int SET_SPEED_NA = 255;
|
||
const SubMaster &sm = *(s.sm);
|
||
|
||
const bool cs_alive = sm.alive("controlsState");
|
||
const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid();
|
||
const auto cs = sm["controlsState"].getControlsState();
|
||
const auto car_state = sm["carState"].getCarState();
|
||
const auto nav_instruction = sm["navInstruction"].getNavInstruction();
|
||
|
||
// Handle older routes where vCruiseCluster is not set
|
||
float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster();
|
||
setSpeed = cs_alive ? v_cruise : SET_SPEED_NA;
|
||
is_cruise_set = setSpeed > 0 && (int)setSpeed != SET_SPEED_NA;
|
||
if (is_cruise_set && !s.scene.is_metric) {
|
||
setSpeed *= KM_TO_MILE;
|
||
}
|
||
|
||
// Handle older routes where vEgoCluster is not set
|
||
v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
|
||
float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo();
|
||
speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
|
||
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
|
||
|
||
auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
|
||
speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||
|
||
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
|
||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
|
||
is_metric = s.scene.is_metric;
|
||
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
|
||
hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals && (turnSignalLeft || turnSignalRight));
|
||
status = s.status;
|
||
|
||
// update engageability/experimental mode button
|
||
experimental_btn->updateState(s);
|
||
|
||
// update DM icon
|
||
auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
|
||
dmActive = dm_state.getIsActiveMode();
|
||
rightHandDM = dm_state.getIsRHD();
|
||
// DM icon transition
|
||
dm_fade_state = std::clamp(dm_fade_state+0.2*(0.5-dmActive), 0.0, 1.0);
|
||
|
||
// hide map settings button for alerts and flip for right hand DM
|
||
if (map_settings_btn->isEnabled()) {
|
||
map_settings_btn->setVisible(!hideBottomIcons);
|
||
main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom);
|
||
}
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
||
p.save();
|
||
|
||
// Header gradient
|
||
QLinearGradient bg(0, UI_HEADER_HEIGHT - (UI_HEADER_HEIGHT / 2.5), 0, UI_HEADER_HEIGHT);
|
||
bg.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.45));
|
||
bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
|
||
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
|
||
|
||
QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
|
||
QString speedStr = QString::number(std::nearbyint(speed));
|
||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–";
|
||
|
||
// Draw outer box + border to contain set speed and speed limit
|
||
const int sign_margin = 12;
|
||
const int us_sign_height = 186;
|
||
const int eu_sign_size = 176;
|
||
|
||
const QSize default_size = {172, 204};
|
||
QSize set_speed_size = default_size;
|
||
if (is_metric || has_eu_speed_limit) set_speed_size.rwidth() = 200;
|
||
if (has_us_speed_limit && speedLimitStr.size() >= 3) set_speed_size.rwidth() = 223;
|
||
|
||
if (has_us_speed_limit) set_speed_size.rheight() += us_sign_height + sign_margin;
|
||
else if (has_eu_speed_limit) set_speed_size.rheight() += eu_sign_size + sign_margin;
|
||
|
||
int top_radius = 32;
|
||
int bottom_radius = has_eu_speed_limit ? 100 : 32;
|
||
|
||
QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
|
||
p.setPen(QPen(whiteColor(75), 6));
|
||
p.setBrush(blackColor(166));
|
||
drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
|
||
|
||
// Draw MAX
|
||
QColor max_color = QColor(0x80, 0xd8, 0xa6, 0xff);
|
||
QColor set_speed_color = whiteColor();
|
||
if (is_cruise_set) {
|
||
if (status == STATUS_DISENGAGED) {
|
||
max_color = whiteColor();
|
||
} else if (status == STATUS_OVERRIDE) {
|
||
max_color = QColor(0x91, 0x9b, 0x95, 0xff);
|
||
} else if (speedLimit > 0) {
|
||
auto interp_color = [=](QColor c1, QColor c2, QColor c3) {
|
||
return speedLimit > 0 ? interpColor(setSpeed, {speedLimit + 5, speedLimit + 15, speedLimit + 25}, {c1, c2, c3}) : c1;
|
||
};
|
||
max_color = interp_color(max_color, QColor(0xff, 0xe4, 0xbf), QColor(0xff, 0xbf, 0xbf));
|
||
set_speed_color = interp_color(set_speed_color, QColor(0xff, 0x95, 0x00), QColor(0xff, 0x00, 0x00));
|
||
}
|
||
} else {
|
||
max_color = QColor(0xa6, 0xa6, 0xa6, 0xff);
|
||
set_speed_color = QColor(0x72, 0x72, 0x72, 0xff);
|
||
}
|
||
p.setFont(InterFont(40, QFont::DemiBold));
|
||
p.setPen(max_color);
|
||
p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("MAX"));
|
||
p.setFont(InterFont(90, QFont::Bold));
|
||
p.setPen(set_speed_color);
|
||
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
|
||
|
||
const QRect sign_rect = set_speed_rect.adjusted(sign_margin, default_size.height(), -sign_margin, -sign_margin);
|
||
// US/Canada (MUTCD style) sign
|
||
if (has_us_speed_limit) {
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(whiteColor());
|
||
p.drawRoundedRect(sign_rect, 24, 24);
|
||
p.setPen(QPen(blackColor(), 6));
|
||
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
|
||
|
||
p.setFont(InterFont(28, QFont::DemiBold));
|
||
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
|
||
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
|
||
p.setFont(InterFont(70, QFont::Bold));
|
||
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
|
||
}
|
||
|
||
// EU (Vienna style) sign
|
||
if (has_eu_speed_limit) {
|
||
p.setPen(Qt::NoPen);
|
||
p.setBrush(whiteColor());
|
||
p.drawEllipse(sign_rect);
|
||
p.setPen(QPen(Qt::red, 20));
|
||
p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
|
||
|
||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||
p.setPen(blackColor());
|
||
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||
}
|
||
|
||
// current speed
|
||
p.setFont(InterFont(176, QFont::Bold));
|
||
drawText(p, rect().center().x(), 210, speedStr);
|
||
p.setFont(InterFont(66));
|
||
drawText(p, rect().center().x(), 290, speedUnit, 200);
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
|
||
QRect real_rect = p.fontMetrics().boundingRect(text);
|
||
real_rect.moveCenter({x, y - real_rect.height() / 2});
|
||
|
||
p.setPen(QColor(0xff, 0xff, 0xff, alpha));
|
||
p.drawText(real_rect.x(), real_rect.bottom(), text);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::initializeGL() {
|
||
CameraWidget::initializeGL();
|
||
qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
|
||
qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR));
|
||
qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER));
|
||
qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));
|
||
|
||
prev_draw_t = millis_since_boot();
|
||
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateFrameMat() {
|
||
CameraWidget::updateFrameMat();
|
||
UIState *s = uiState();
|
||
int w = width(), h = height();
|
||
|
||
s->fb_w = w;
|
||
s->fb_h = h;
|
||
|
||
// Apply transformation such that video pixel coordinates match video
|
||
// 1) Put (0, 0) in the middle of the video
|
||
// 2) Apply same scaling as video
|
||
// 3) Put (0, 0) in top left corner of video
|
||
s->car_space_transform.reset();
|
||
s->car_space_transform.translate(w / 2 - x_offset, h / 2 - y_offset)
|
||
.scale(zoom, zoom)
|
||
.translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
|
||
painter.save();
|
||
|
||
SubMaster &sm = *(s->sm);
|
||
|
||
// lanelines
|
||
for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) {
|
||
if (customColors != 0) {
|
||
painter.setBrush(std::get<3>(themeConfiguration[customColors]).begin()->second);
|
||
} else {
|
||
painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(scene.lane_line_probs[i], 0.0, 0.7)));
|
||
}
|
||
painter.drawPolygon(scene.lane_line_vertices[i]);
|
||
}
|
||
|
||
// road edges
|
||
for (int i = 0; i < std::size(scene.road_edge_vertices); ++i) {
|
||
if (customColors != 0) {
|
||
painter.setBrush(std::get<3>(themeConfiguration[customColors]).begin()->second);
|
||
} else {
|
||
painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0)));
|
||
}
|
||
painter.drawPolygon(scene.road_edge_vertices[i]);
|
||
}
|
||
|
||
// paint path
|
||
QLinearGradient bg(0, height(), 0, 0);
|
||
if (sm["controlsState"].getControlsState().getExperimentalMode() || scene.acceleration_path) {
|
||
// The first half of track_vertices are the points for the right side of the path
|
||
// and the indices match the positions of accel from uiPlan
|
||
const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel();
|
||
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration_const.size());
|
||
|
||
// Copy of the acceleration vector
|
||
std::vector<float> acceleration;
|
||
for (int i = 0; i < acceleration_const.size(); i++) {
|
||
acceleration.push_back(acceleration_const[i]);
|
||
}
|
||
|
||
for (int i = 0; i < max_len; ++i) {
|
||
// Some points are out of frame
|
||
if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue;
|
||
|
||
// Flip so 0 is bottom of frame
|
||
float lin_grad_point = (height() - scene.track_vertices[i].y()) / height();
|
||
|
||
// If acceleration is between -0.2 and 0.2, resort to the theme color
|
||
if (std::abs(acceleration[i]) < 0.2 && (customColors != 0)) {
|
||
const auto &colorMap = std::get<3>(themeConfiguration[customColors]);
|
||
for (const auto &[position, brush] : colorMap) {
|
||
bg.setColorAt(position, brush.color());
|
||
}
|
||
} else {
|
||
// speed up: 120, slow down: 0
|
||
float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0);
|
||
// FIXME: painter.drawPolygon can be slow if hue is not rounded
|
||
path_hue = int(path_hue * 100 + 0.5) / 100;
|
||
|
||
float saturation = fmin(fabs(acceleration[i] * 1.5), 1);
|
||
float lightness = util::map_val(saturation, 0.0f, 1.0f, 0.95f, 0.62f); // lighter when grey
|
||
float alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, 0.4f, 0.0f); // matches previous alpha fade
|
||
bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha));
|
||
|
||
// Skip a point, unless next is last
|
||
i += (i + 2) < max_len ? 1 : 0;
|
||
}
|
||
}
|
||
|
||
} else if (customColors != 0) {
|
||
const auto &colorMap = std::get<3>(themeConfiguration[customColors]);
|
||
for (const auto &[position, brush] : colorMap) {
|
||
bg.setColorAt(position, brush.color());
|
||
}
|
||
} else {
|
||
bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4));
|
||
bg.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.35));
|
||
bg.setColorAt(1.0, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.0));
|
||
}
|
||
|
||
painter.setBrush(bg);
|
||
painter.drawPolygon(scene.track_vertices);
|
||
|
||
// Create new path with track vertices and track edge vertices
|
||
QPainterPath path;
|
||
path.addPolygon(scene.track_vertices);
|
||
path.addPolygon(scene.track_edge_vertices);
|
||
|
||
// Paint path edges
|
||
QLinearGradient pe(0, height(), 0, 0);
|
||
if (alwaysOnLateralActive) {
|
||
pe.setColorAt(0.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 1.0));
|
||
pe.setColorAt(0.5, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.5));
|
||
pe.setColorAt(1.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.1));
|
||
} else if (conditionalStatus == 1 || conditionalStatus == 3) {
|
||
pe.setColorAt(0.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 1.0));
|
||
pe.setColorAt(0.5, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.5));
|
||
pe.setColorAt(1.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.1));
|
||
} else if (experimentalMode) {
|
||
pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0));
|
||
pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5));
|
||
pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1));
|
||
} else if (scene.navigate_on_openpilot) {
|
||
pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0));
|
||
pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5));
|
||
pe.setColorAt(1.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.1));
|
||
} else if (customColors != 0) {
|
||
const auto &colorMap = std::get<3>(themeConfiguration[customColors]);
|
||
for (const auto &[position, brush] : colorMap) {
|
||
QColor darkerColor = brush.color().darker(120);
|
||
pe.setColorAt(position, darkerColor);
|
||
}
|
||
} else {
|
||
pe.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 1.0));
|
||
pe.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.5));
|
||
pe.setColorAt(1.0, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.1));
|
||
}
|
||
|
||
painter.setBrush(pe);
|
||
painter.drawPath(path);
|
||
|
||
// Paint blindspot path
|
||
if (scene.blind_spot_path) {
|
||
QLinearGradient bs(0, height(), 0, 0);
|
||
if (blindSpotLeft || blindSpotRight) {
|
||
bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6));
|
||
bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4));
|
||
bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2));
|
||
}
|
||
|
||
painter.setBrush(bs);
|
||
if (blindSpotLeft) {
|
||
painter.drawPolygon(scene.track_adjacent_vertices[4]);
|
||
}
|
||
if (blindSpotRight) {
|
||
painter.drawPolygon(scene.track_adjacent_vertices[5]);
|
||
}
|
||
}
|
||
|
||
painter.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) {
|
||
painter.save();
|
||
|
||
// base icon
|
||
int offset = UI_BORDER_SIZE + btn_size / 2;
|
||
offset += alwaysOnLateral || conditionalExperimental ? 25 : 0;
|
||
int x = rightHandDM ? width() - offset : offset;
|
||
int y = height() - offset;
|
||
float opacity = dmActive ? 0.65 : 0.2;
|
||
drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
|
||
|
||
// face
|
||
QPointF face_kpts_draw[std::size(default_face_kpts_3d)];
|
||
float kp;
|
||
for (int i = 0; i < std::size(default_face_kpts_3d); ++i) {
|
||
kp = (scene.face_kpts_draw[i].v[2] - 8) / 120 + 1.0;
|
||
face_kpts_draw[i] = QPointF(scene.face_kpts_draw[i].v[0] * kp + x, scene.face_kpts_draw[i].v[1] * kp + y);
|
||
}
|
||
|
||
painter.setPen(QPen(QColor::fromRgbF(1.0, 1.0, 1.0, opacity), 5.2, Qt::SolidLine, Qt::RoundCap));
|
||
painter.drawPolyline(face_kpts_draw, std::size(default_face_kpts_3d));
|
||
|
||
// tracking arcs
|
||
const int arc_l = 133;
|
||
const float arc_t_default = 6.7;
|
||
const float arc_t_extend = 12.0;
|
||
QColor arc_color = QColor::fromRgbF(0.545 - 0.445 * s->engaged(),
|
||
0.545 + 0.4 * s->engaged(),
|
||
0.545 - 0.285 * s->engaged(),
|
||
0.4 * (1.0 - dm_fade_state));
|
||
float delta_x = -scene.driver_pose_sins[1] * arc_l / 2;
|
||
float delta_y = -scene.driver_pose_sins[0] * arc_l / 2;
|
||
painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[1] * 5.0), Qt::SolidLine, Qt::RoundCap));
|
||
painter.drawArc(QRectF(std::fmin(x + delta_x, x), y - arc_l / 2, fabs(delta_x), arc_l), (scene.driver_pose_sins[1]>0 ? 90 : -90) * 16, 180 * 16);
|
||
painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[0] * 5.0), Qt::SolidLine, Qt::RoundCap));
|
||
painter.drawArc(QRectF(x - arc_l / 2, std::fmin(y + delta_y, y), arc_l, fabs(delta_y)), (scene.driver_pose_sins[0]>0 ? 0 : 180) * 16, 180 * 16);
|
||
|
||
painter.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) {
|
||
painter.save();
|
||
|
||
const float speedBuff = customColors ? 25. : 10.; // Make the center of the chevron appear sooner if a custom theme is active
|
||
const float leadBuff = customColors ? 100. : 40.; // Make the center of the chevron appear sooner if a custom theme is active
|
||
const float d_rel = lead_data.getDRel();
|
||
const float v_rel = lead_data.getVRel();
|
||
|
||
float fillAlpha = 0;
|
||
if (d_rel < leadBuff) {
|
||
fillAlpha = 255 * (1.0 - (d_rel / leadBuff));
|
||
if (v_rel < 0) {
|
||
fillAlpha += 255 * (-1 * (v_rel / speedBuff));
|
||
}
|
||
fillAlpha = (int)(fmin(fillAlpha, 255));
|
||
}
|
||
|
||
float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
|
||
float x = std::clamp((float)vd.x(), 0.f, width() - sz / 2);
|
||
float y = std::fmin(height() - sz * .6, (float)vd.y());
|
||
|
||
float g_xo = sz / 5;
|
||
float g_yo = sz / 10;
|
||
|
||
QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
|
||
painter.setBrush(QColor(218, 202, 37, 255));
|
||
painter.drawPolygon(glow, std::size(glow));
|
||
|
||
// chevron
|
||
QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
|
||
if (customColors != 0) {
|
||
painter.setBrush(std::get<3>(themeConfiguration[customColors]).begin()->second);
|
||
} else {
|
||
painter.setBrush(redColor(fillAlpha));
|
||
}
|
||
painter.drawPolygon(chevron, std::size(chevron));
|
||
|
||
painter.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::paintGL() {
|
||
UIState *s = uiState();
|
||
SubMaster &sm = *(s->sm);
|
||
const double start_draw_t = millis_since_boot();
|
||
const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2();
|
||
|
||
// draw camera frame
|
||
{
|
||
std::lock_guard lk(frame_lock);
|
||
|
||
if (frames.empty()) {
|
||
if (skip_frame_count > 0) {
|
||
skip_frame_count--;
|
||
qDebug() << "skipping frame, not ready";
|
||
return;
|
||
}
|
||
} else {
|
||
// skip drawing up to this many frames if we're
|
||
// missing camera frames. this smooths out the
|
||
// transitions from the narrow and wide cameras
|
||
skip_frame_count = 5;
|
||
}
|
||
|
||
// Wide or narrow cam dependent on speed
|
||
bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD);
|
||
if (has_wide_cam && cameraView == 0) {
|
||
float v_ego = sm["carState"].getCarState().getVEgo();
|
||
if ((v_ego < 10) || available_streams.size() == 1) {
|
||
wide_cam_requested = true;
|
||
} else if (v_ego > 15) {
|
||
wide_cam_requested = false;
|
||
}
|
||
wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode();
|
||
// for replay of old routes, never go to widecam
|
||
wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
|
||
}
|
||
CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER :
|
||
cameraView == 2 || wide_cam_requested ? VISION_STREAM_WIDE_ROAD :
|
||
VISION_STREAM_ROAD);
|
||
|
||
s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
|
||
if (s->scene.calibration_valid) {
|
||
auto calib = s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;
|
||
CameraWidget::updateCalibration(calib);
|
||
} else {
|
||
CameraWidget::updateCalibration(DEFAULT_CALIBRATION);
|
||
}
|
||
CameraWidget::setFrameId(model.getFrameId());
|
||
CameraWidget::paintGL();
|
||
}
|
||
|
||
QPainter painter(this);
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
painter.setPen(Qt::NoPen);
|
||
|
||
if (s->scene.world_objects_visible) {
|
||
update_model(s, model, sm["uiPlan"].getUiPlan());
|
||
drawLaneLines(painter, s);
|
||
|
||
if (s->scene.longitudinal_control && sm.rcv_frame("radarState") > s->scene.started_frame) {
|
||
auto radar_state = sm["radarState"].getRadarState();
|
||
update_leads(s, radar_state, model.getPosition());
|
||
auto lead_one = radar_state.getLeadOne();
|
||
auto lead_two = radar_state.getLeadTwo();
|
||
if (lead_one.getStatus()) {
|
||
drawLead(painter, lead_one, s->scene.lead_vertices[0]);
|
||
}
|
||
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
|
||
drawLead(painter, lead_two, s->scene.lead_vertices[1]);
|
||
}
|
||
}
|
||
}
|
||
|
||
// DMoji
|
||
if (!hideBottomIcons && (sm.rcv_frame("driverStateV2") > s->scene.started_frame)) {
|
||
update_dmonitoring(s, sm["driverStateV2"].getDriverStateV2(), dm_fade_state, rightHandDM);
|
||
drawDriverState(painter, s);
|
||
}
|
||
|
||
drawHud(painter);
|
||
|
||
double cur_draw_t = millis_since_boot();
|
||
double dt = cur_draw_t - prev_draw_t;
|
||
double fps = fps_filter.update(1. / dt * 1000);
|
||
if (fps < 15) {
|
||
LOGW("slow frame rate: %.2f fps", fps);
|
||
}
|
||
prev_draw_t = cur_draw_t;
|
||
|
||
// publish debug msg
|
||
MessageBuilder msg;
|
||
auto m = msg.initEvent().initUiDebug();
|
||
m.setDrawTimeMillis(cur_draw_t - start_draw_t);
|
||
pm->send("uiDebug", msg);
|
||
|
||
// Update FrogPilot widgets
|
||
updateFrogPilotWidgets(painter);
|
||
}
|
||
|
||
void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
|
||
CameraWidget::showEvent(event);
|
||
|
||
ui_update_params(uiState());
|
||
prev_draw_t = millis_since_boot();
|
||
}
|
||
|
||
// FrogPilot widgets
|
||
void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
|
||
bottom_layout = new QHBoxLayout();
|
||
|
||
QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum);
|
||
bottom_layout->addItem(spacer);
|
||
|
||
map_settings_btn_bottom = new MapSettingsButton(this);
|
||
bottom_layout->addWidget(map_settings_btn_bottom);
|
||
|
||
main_layout->addLayout(bottom_layout);
|
||
|
||
// Custom themes configuration
|
||
themeConfiguration = {
|
||
{1, {"frog_theme", 4, QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))},
|
||
{0.5, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.5))},
|
||
{1.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.1))}}}},
|
||
{2, {"tesla_theme", 4, QColor(0, 72, 255, 255), {{0.0, QBrush(QColor::fromHslF(223 / 360., 1.0, 0.5, 0.9))},
|
||
{0.5, QBrush(QColor::fromHslF(223 / 360., 1.0, 0.5, 0.5))},
|
||
{1.0, QBrush(QColor::fromHslF(223 / 360., 1.0, 0.5, 0.1))}}}},
|
||
{3, {"stalin_theme", 6, QColor(255, 0, 0, 255), {{0.0, QBrush(QColor::fromHslF(0 / 360., 1.0, 0.5, 0.9))},
|
||
{0.5, QBrush(QColor::fromHslF(0 / 360., 1.0, 0.5, 0.5))},
|
||
{1.0, QBrush(QColor::fromHslF(0 / 360., 1.0, 0.5, 0.1))}}}}
|
||
};
|
||
|
||
// Initialize the timer for the turn signal animation
|
||
animationTimer = new QTimer(this);
|
||
connect(animationTimer, &QTimer::timeout, this, [this] {
|
||
animationFrameIndex = (animationFrameIndex + 1) % totalFrames;
|
||
});
|
||
}
|
||
|
||
void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
|
||
alwaysOnLateral = scene.always_on_lateral;
|
||
alwaysOnLateralActive = scene.always_on_lateral_active;
|
||
|
||
blindSpotLeft = scene.blind_spot_left;
|
||
blindSpotRight = scene.blind_spot_right;
|
||
|
||
cameraView = scene.camera_view;
|
||
|
||
conditionalExperimental = scene.conditional_experimental;
|
||
conditionalSpeed = scene.conditional_speed;
|
||
conditionalSpeedLead = scene.conditional_speed_lead;
|
||
conditionalStatus = scene.conditional_status;
|
||
|
||
customColors = scene.custom_colors;
|
||
|
||
experimentalMode = scene.experimental_mode;
|
||
|
||
turnSignalLeft = scene.turn_signal_left;
|
||
turnSignalRight = scene.turn_signal_right;
|
||
|
||
if (alwaysOnLateral || conditionalExperimental) {
|
||
drawStatusBar(p);
|
||
}
|
||
|
||
if (customSignals && (turnSignalLeft || turnSignalRight)) {
|
||
if (!animationTimer->isActive()) {
|
||
animationTimer->start(totalFrames * 11); // 440 milliseconds per loop; syncs up perfectly with my 2019 Lexus ES 350 turn signal clicks
|
||
}
|
||
drawTurnSignals(p);
|
||
} else if (animationTimer->isActive()) {
|
||
animationTimer->stop();
|
||
}
|
||
|
||
map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled());
|
||
if (map_settings_btn_bottom->isEnabled()) {
|
||
map_settings_btn_bottom->setVisible(!hideBottomIcons);
|
||
bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight);
|
||
}
|
||
|
||
// Update the turn signal animation images upon toggle change
|
||
if (customSignals != scene.custom_signals) {
|
||
customSignals = scene.custom_signals;
|
||
|
||
QString theme_path = QString("../frogpilot/assets/custom_themes/%1/images").arg(themeConfiguration.find(customSignals) != themeConfiguration.end() ?
|
||
std::get<0>(themeConfiguration[customSignals]) : "");
|
||
|
||
QStringList imagePaths;
|
||
int availableImages = std::get<1>(themeConfiguration[customSignals]);
|
||
|
||
for (int i = 1; i <= totalFrames; ++i) {
|
||
int imageIndex = ((i - 1) % availableImages) + 1;
|
||
QString imagePath = theme_path + QString("/turn_signal_%1.png").arg(imageIndex);
|
||
imagePaths.push_back(imagePath);
|
||
}
|
||
|
||
signalImgVector.clear();
|
||
signalImgVector.reserve(2 * imagePaths.size()); // Reserve space for both regular and flipped images
|
||
for (const QString &imagePath : imagePaths) {
|
||
QPixmap pixmap(imagePath);
|
||
signalImgVector.push_back(pixmap); // Regular image
|
||
signalImgVector.push_back(pixmap.transformed(QTransform().scale(-1, 1))); // Flipped image
|
||
}
|
||
|
||
signalImgVector.push_back(QPixmap(theme_path + "/turn_signal_1_red.png")); // Regular blindspot image
|
||
signalImgVector.push_back(QPixmap(theme_path + "/turn_signal_1_red.png").transformed(QTransform().scale(-1, 1))); // Flipped blindspot image
|
||
}
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
|
||
p.save();
|
||
|
||
// Variable declarations
|
||
static QElapsedTimer timer;
|
||
static QString lastShownStatus;
|
||
|
||
QString newStatus;
|
||
|
||
static bool displayStatusText = false;
|
||
|
||
constexpr qreal fadeDuration = 1500.0;
|
||
constexpr qreal textDuration = 5000.0;
|
||
|
||
// Draw status bar
|
||
QRect currentRect = rect();
|
||
QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100);
|
||
p.setBrush(QColor(0, 0, 0, 150));
|
||
p.setOpacity(1.0);
|
||
p.drawRoundedRect(statusBarRect, 30, 30);
|
||
|
||
std::map<int, QString> conditionalStatusMap = {
|
||
{0, "Conditional Experimental Mode ready"},
|
||
{1, "Conditional Experimental overridden"},
|
||
{2, "Experimental Mode manually activated"},
|
||
{3, "Conditional Experimental overridden"},
|
||
{4, "Experimental Mode manually activated"},
|
||
{5, "Experimental Mode activated for" + (mapOpen ? " intersection" : QString(" upcoming intersection"))},
|
||
{6, "Experimental Mode activated for" + (mapOpen ? " turn" : QString(" upcoming turn"))},
|
||
{7, "Experimental Mode activated due to" + (mapOpen ? "SLC" : QString(" no speed limit set"))},
|
||
{8, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))},
|
||
{9, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))},
|
||
{10, "Experimental Mode activated for slower lead"},
|
||
{11, "Experimental Mode activated for turn" + (mapOpen ? "" : QString(" / lane change"))},
|
||
{12, "Experimental Mode activated for curve"},
|
||
{13, "Experimental Mode activated for stop" + (mapOpen ? "" : QString(" sign / stop light"))},
|
||
};
|
||
|
||
// Update status text
|
||
if (alwaysOnLateralActive) {
|
||
newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable");
|
||
} else if (conditionalExperimental) {
|
||
newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
|
||
}
|
||
|
||
// Check if status has changed
|
||
if (newStatus != lastShownStatus) {
|
||
displayStatusText = true;
|
||
lastShownStatus = newStatus;
|
||
timer.restart();
|
||
} else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) {
|
||
displayStatusText = false;
|
||
}
|
||
|
||
// Configure the text
|
||
p.setFont(InterFont(40, QFont::Bold));
|
||
p.setPen(Qt::white);
|
||
p.setRenderHint(QPainter::TextAntialiasing);
|
||
|
||
// Calculate text opacity
|
||
static qreal statusTextOpacity;
|
||
int elapsed = timer.elapsed();
|
||
if (displayStatusText) {
|
||
statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0);
|
||
}
|
||
|
||
// Draw the status text
|
||
p.setOpacity(statusTextOpacity);
|
||
QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
|
||
textRect.moveBottom(statusBarRect.bottom() - 50);
|
||
p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus);
|
||
|
||
p.restore();
|
||
}
|
||
|
||
void AnnotatedCameraWidget::drawTurnSignals(QPainter &p) {
|
||
// Declare the turn signal size
|
||
constexpr int signalHeight = 480;
|
||
constexpr int signalWidth = 360;
|
||
|
||
// Enable Antialiasing
|
||
p.setRenderHint(QPainter::Antialiasing);
|
||
|
||
// Calculate the vertical position for the turn signals
|
||
int baseYPosition = (height() - signalHeight) / 2 + (alwaysOnLateral || conditionalExperimental || roadNameUI ? 225 : 300);
|
||
// Calculate the x-coordinates for the turn signals
|
||
int leftSignalXPosition = 75 + width() - signalWidth - 300 * (blindSpotLeft ? 0 : animationFrameIndex);
|
||
int rightSignalXPosition = -75 + 300 * (blindSpotRight ? 0 : animationFrameIndex);
|
||
|
||
// Draw the turn signals
|
||
if (animationFrameIndex < signalImgVector.size()) {
|
||
auto drawSignal = [&](bool signalActivated, int xPosition, bool flip, bool blindspot) {
|
||
if (signalActivated) {
|
||
// Get the appropriate image from the signalImgVector
|
||
int uniqueImages = signalImgVector.size() / 4; // Each image has a regular, flipped, and two blindspot versions
|
||
int index = (blindspot ? 2 * uniqueImages : 2 * animationFrameIndex % totalFrames) + (flip ? 1 : 0);
|
||
QPixmap &signal = signalImgVector[index];
|
||
p.drawPixmap(xPosition, baseYPosition, signalWidth, signalHeight, signal);
|
||
}
|
||
};
|
||
|
||
// Display the animation based on which signal is activated
|
||
drawSignal(turnSignalLeft, leftSignalXPosition, false, blindSpotLeft);
|
||
drawSignal(turnSignalRight, rightSignalXPosition, true, blindSpotRight);
|
||
}
|
||
}
|