sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
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.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
9260 Commits
122 Branches
82 Tags
5.1 GiB
 
 
 
 
 
 
Tag: Branch: Tree: ee86c385b3
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'ee86c385b3'
${ noResults }
openpilot_comma/selfdrive/ui/qt/onroad.cc

762 lines
28 KiB
Raw Blame History

#include "selfdrive/ui/qt/onroad.h"
#include <cmath>
#include <QDebug>
#include "common/timing.h"
#include "selfdrive/ui/qt/util.h"
#ifdef ENABLE_MAPS
#include "selfdrive/ui/qt/maps/map.h"
#include "selfdrive/ui/qt/maps/map_helpers.h"
#endif
OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setMargin(bdr_s);
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);
}
void OnroadWindow::updateState(const UIState &s) {
QColor bgColor = bg_colors[s.status];
Alert alert = Alert::get(*(s.sm), s.scene.started_frame);
if (s.sm->updated("controlsState") || !alert.equal({})) {
if (alert.type == "controlsUnresponsive") {
bgColor = bg_colors[STATUS_ALERT];
} else if (alert.type == "controlsUnresponsivePermanent") {
bgColor = bg_colors[STATUS_DISENGAGED];
}
alerts->updateAlert(alert, bgColor);
}
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) {
if (map != nullptr) {
bool sidebarVisible = geometry().x() > 0;
map->setVisible(!sidebarVisible && !map->isVisible());
}
// propagation event to parent(HomeWindow)
QWidget::mousePressEvent(e);
}
void OnroadWindow::offroadTransition(bool offroad) {
#ifdef ENABLE_MAPS
if (!offroad) {
if (map == nullptr && (uiState()->prime_type || !MAPBOX_TOKEN.isEmpty())) {
MapWindow * m = new MapWindow(get_mapbox_settings());
map = m;
QObject::connect(uiState(), &UIState::offroadTransition, m, &MapWindow::offroadTransition);
m->setFixedWidth(topWidget(this)->width() / 2);
split->insertWidget(0, m);
// Make map visible after adding to split
m->offroadTransition(offroad);
}
}
#endif
alerts->updateAlert({}, bg);
}
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, const QColor &color) {
if (!alert.equal(a) || color != bg) {
alert = a;
bg = color;
update();
}
}
void OnroadAlerts::paintEvent(QPaintEvent *event) {
if (alert.size == cereal::ControlsState::AlertSize::NONE) {
return;
}
static std::map<cereal::ControlsState::AlertSize, const int> alert_sizes = {
{cereal::ControlsState::AlertSize::SMALL, 271},
{cereal::ControlsState::AlertSize::MID, 420},
{cereal::ControlsState::AlertSize::FULL, height()},
};
int h = alert_sizes[alert.size];
QRect r = QRect(0, height() - h, width(), h);
QPainter p(this);
// draw background + gradient
p.setPen(Qt::NoPen);
p.setCompositionMode(QPainter::CompositionMode_SourceOver);
p.setBrush(QBrush(bg));
p.drawRect(r);
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.fillRect(r, g);
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) {
configFont(p, "Inter", 74, "SemiBold");
p.drawText(r, Qt::AlignCenter, alert.text1);
} else if (alert.size == cereal::ControlsState::AlertSize::MID) {
configFont(p, "Inter", 88, "Bold");
p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1);
configFont(p, "Inter", 66, "Regular");
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;
configFont(p, "Inter", l ? 132 : 177, "Bold");
p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1);
configFont(p, "Inter", 88, "Regular");
p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2);
}
}
ExperimentalButton::ExperimentalButton(QWidget *parent) : QPushButton(parent) {
setVisible(false);
setFixedSize(btn_size, btn_size);
setCheckable(true);
params = Params();
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::toggled, [=](bool checked) {
params.putBool("ExperimentalMode", checked);
});
}
void ExperimentalButton::updateState(const UIState &s) {
const SubMaster &sm = *(s.sm);
// button is "visible" if engageable or enabled
const auto cs = sm["controlsState"].getControlsState();
setVisible(cs.getEngageable() || cs.getEnabled());
// button is "checked" if experimental mode is enabled
setChecked(sm["controlsState"].getControlsState().getExperimentalMode());
// disable button when experimental mode is not available, or has not been confirmed for the first time
const auto cp = sm["carParams"].getCarParams();
const bool experimental_mode_available = cp.getExperimentalLongitudinalAvailable() ? params.getBool("ExperimentalLongitudinalEnabled") : cp.getOpenpilotLongitudinalControl();
setEnabled(params.getBool("ExperimentalModeConfirmed") && experimental_mode_available);
}
void ExperimentalButton::paintEvent(QPaintEvent *event) {
QPainter p(this);
p.setRenderHint(QPainter::Antialiasing);
QPoint center(btn_size / 2, btn_size / 2);
QPixmap img = isChecked() ? experimental_img : engage_img;
p.setOpacity(1.0);
p.setPen(Qt::NoPen);
p.setBrush(QColor(0, 0, 0, 166));
p.drawEllipse(center, btn_size / 2, btn_size / 2);
p.setOpacity(isDown() ? 0.8 : 1.0);
p.drawPixmap((btn_size - img_size) / 2, (btn_size - img_size) / 2, img);
}
AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) {
pm = std::make_unique<PubMaster, const std::initializer_list<const char *>>({"uiDebug"});
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setMargin(bdr_s);
main_layout->setSpacing(0);
experimental_btn = new ExperimentalButton(this);
main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
}
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();
// Handle older routes where vCruiseCluster is not set
float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster();
float set_speed = cs_alive ? v_cruise : SET_SPEED_NA;
bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA;
if (cruise_set && !s.scene.is_metric) {
set_speed *= KM_TO_MILE;
}
// Handle older routes where vEgoCluster is not set
float v_ego;
if (sm["carState"].getCarState().getVEgoCluster() == 0.0 && !v_ego_cluster_seen) {
v_ego = sm["carState"].getCarState().getVEgo();
} else {
v_ego = sm["carState"].getCarState().getVEgoCluster();
v_ego_cluster_seen = true;
}
float cur_speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign();
float speed_limit = nav_alive ? sm["navInstruction"].getNavInstruction().getSpeedLimit() : 0.0;
speed_limit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
setProperty("speedLimit", speed_limit);
setProperty("has_us_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
setProperty("has_eu_speed_limit", nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
setProperty("is_cruise_set", cruise_set);
setProperty("is_metric", s.scene.is_metric);
setProperty("speed", cur_speed);
setProperty("setSpeed", set_speed);
setProperty("speedUnit", s.scene.is_metric ? tr("km/h") : tr("mph"));
setProperty("hideDM", (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE));
setProperty("status", s.status);
// update engageability/experimental mode button
experimental_btn->updateState(s);
// update DM icons at 2Hz
if (sm.frame % (UI_FREQ / 2) == 0) {
setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode());
setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD());
}
// DM icon transition
dm_fade_state = fmax(0.0, fmin(1.0, dm_fade_state+0.2*(0.5-(float)(dmActive))));
}
void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.save();
// Header gradient
QLinearGradient bg(0, header_h - (header_h / 2.5), 0, header_h);
bg.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.45));
bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
p.fillRect(0, 0, width(), header_h, 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
int default_rect_width = 172;
int rect_width = default_rect_width;
if (is_metric || has_eu_speed_limit) rect_width = 200;
if (has_us_speed_limit && speedLimitStr.size() >= 3) rect_width = 223;
int rect_height = 204;
if (has_us_speed_limit) rect_height = 402;
else if (has_eu_speed_limit) rect_height = 392;
int top_radius = 32;
int bottom_radius = has_eu_speed_limit ? 100 : 32;
QRect set_speed_rect(60 + default_rect_width / 2 - rect_width / 2, 45, rect_width, rect_height);
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
if (is_cruise_set) {
if (status == STATUS_DISENGAGED) {
p.setPen(whiteColor());
} else if (status == STATUS_OVERRIDE) {
p.setPen(QColor(0x91, 0x9b, 0x95, 0xff));
} else if (speedLimit > 0) {
p.setPen(interpColor(
setSpeed,
{speedLimit + 5, speedLimit + 15, speedLimit + 25},
{QColor(0x80, 0xd8, 0xa6, 0xff), QColor(0xff, 0xe4, 0xbf, 0xff), QColor(0xff, 0xbf, 0xbf, 0xff)}
));
} else {
p.setPen(QColor(0x80, 0xd8, 0xa6, 0xff));
}
} else {
p.setPen(QColor(0xa6, 0xa6, 0xa6, 0xff));
}
configFont(p, "Inter", 40, "SemiBold");
QRect max_rect = getTextRect(p, Qt::AlignCenter, tr("MAX"));
max_rect.moveCenter({set_speed_rect.center().x(), 0});
max_rect.moveTop(set_speed_rect.top() + 27);
p.drawText(max_rect, Qt::AlignCenter, tr("MAX"));
// Draw set speed
if (is_cruise_set) {
if (speedLimit > 0 && status != STATUS_DISENGAGED && status != STATUS_OVERRIDE) {
p.setPen(interpColor(
setSpeed,
{speedLimit + 5, speedLimit + 15, speedLimit + 25},
{whiteColor(), QColor(0xff, 0x95, 0x00, 0xff), QColor(0xff, 0x00, 0x00, 0xff)}
));
} else {
p.setPen(whiteColor());
}
} else {
p.setPen(QColor(0x72, 0x72, 0x72, 0xff));
}
configFont(p, "Inter", 90, "Bold");
QRect speed_rect = getTextRect(p, Qt::AlignCenter, setSpeedStr);
speed_rect.moveCenter({set_speed_rect.center().x(), 0});
speed_rect.moveTop(set_speed_rect.top() + 77);
p.drawText(speed_rect, Qt::AlignCenter, setSpeedStr);
// US/Canada (MUTCD style) sign
if (has_us_speed_limit) {
const int border_width = 6;
const int sign_width = rect_width - 24;
const int sign_height = 186;
// White outer square
QRect sign_rect_outer(set_speed_rect.left() + 12, set_speed_rect.bottom() - 11 - sign_height, sign_width, sign_height);
p.setPen(Qt::NoPen);
p.setBrush(whiteColor());
p.drawRoundedRect(sign_rect_outer, 24, 24);
// Smaller white square with black border
QRect sign_rect(sign_rect_outer.left() + 1.5 * border_width, sign_rect_outer.top() + 1.5 * border_width, sign_width - 3 * border_width, sign_height - 3 * border_width);
p.setPen(QPen(blackColor(), border_width));
p.setBrush(whiteColor());
p.drawRoundedRect(sign_rect, 16, 16);
// "SPEED"
configFont(p, "Inter", 28, "SemiBold");
QRect text_speed_rect = getTextRect(p, Qt::AlignCenter, tr("SPEED"));
text_speed_rect.moveCenter({sign_rect.center().x(), 0});
text_speed_rect.moveTop(sign_rect_outer.top() + 22);
p.drawText(text_speed_rect, Qt::AlignCenter, tr("SPEED"));
// "LIMIT"
QRect text_limit_rect = getTextRect(p, Qt::AlignCenter, tr("LIMIT"));
text_limit_rect.moveCenter({sign_rect.center().x(), 0});
text_limit_rect.moveTop(sign_rect_outer.top() + 51);
p.drawText(text_limit_rect, Qt::AlignCenter, tr("LIMIT"));
// Speed limit value
configFont(p, "Inter", 70, "Bold");
QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr);
speed_limit_rect.moveCenter({sign_rect.center().x(), 0});
speed_limit_rect.moveTop(sign_rect_outer.top() + 85);
p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr);
}
// EU (Vienna style) sign
if (has_eu_speed_limit) {
int outer_radius = 176 / 2;
int inner_radius_1 = outer_radius - 6; // White outer border
int inner_radius_2 = inner_radius_1 - 20; // Red circle
// Draw white circle with red border
QPoint center(set_speed_rect.center().x() + 1, set_speed_rect.top() + 204 + outer_radius);
p.setPen(Qt::NoPen);
p.setBrush(whiteColor());
p.drawEllipse(center, outer_radius, outer_radius);
p.setBrush(QColor(255, 0, 0, 255));
p.drawEllipse(center, inner_radius_1, inner_radius_1);
p.setBrush(whiteColor());
p.drawEllipse(center, inner_radius_2, inner_radius_2);
// Speed limit value
int font_size = (speedLimitStr.size() >= 3) ? 60 : 70;
configFont(p, "Inter", font_size, "Bold");
QRect speed_limit_rect = getTextRect(p, Qt::AlignCenter, speedLimitStr);
speed_limit_rect.moveCenter(center);
p.setPen(blackColor());
p.drawText(speed_limit_rect, Qt::AlignCenter, speedLimitStr);
}
// current speed
configFont(p, "Inter", 176, "Bold");
drawText(p, rect().center().x(), 210, speedStr);
configFont(p, "Inter", 66, "Regular");
drawText(p, rect().center().x(), 290, speedUnit, 200);
p.restore();
}
// Window that shows camera view and variety of
// info drawn on top
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
QRect real_rect = getTextRect(p, 0, 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::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) {
p.setOpacity(1.0); // bg dictates opacity of ellipse
p.setPen(Qt::NoPen);
p.setBrush(bg);
p.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size);
p.setOpacity(opacity);
p.drawPixmap(x - img.size().width() / 2, y - img.size().height() / 2, img);
}
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]);
}
float interp1d(float value, float start_min, float start_max, float end_min, float end_max) {
value = std::max(start_min, std::min(start_max, value));
float factor = (value - start_min) / (start_max - start_min);
return end_min + factor * (end_max - end_min);
}
void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
painter.save();
const UIScene &scene = s->scene;
SubMaster &sm = *(s->sm);
// lanelines
for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) {
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) {
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
// double t = millis_since_boot();
QLinearGradient bg(0, height(), 0, 0);
if (sm["controlsState"].getControlsState().getExperimentalMode()) {
int track_vertices_len = scene.track_vertices.length();
assert(track_vertices_len % 2 == 0);
QVector<QPointF> right_points = scene.track_vertices.mid(0, track_vertices_len / 2);
// Filter out points out of frame
QVector<QPointF> valid_points;
std::copy_if(right_points.begin(), right_points.end(), std::back_inserter(valid_points),
[&](const QPointF &point) { return point.y() >= 0 && point.y() <= height(); });
qDebug() << "valid_points len:" << valid_points.length() << "right_points len:" << right_points.length();
int final_idx = 0;
if (!valid_points.isEmpty()) {
for (int p = 0; p < MAX_PATH_POINTS; p++) {
int i = (p / (MAX_PATH_POINTS - 1)) * (valid_points.length() - 1);
final_idx = i;
qDebug() << "i:" << i << "len points:" << valid_points.length();
const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel();
if (i >= acceleration.size()) break;
// flip so 0 is bottom of frame
float lin_grad_point = (height() - valid_points[i].y()) / height();
qDebug() << "lin_grad_point:" << lin_grad_point;
// 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 = interp1d(saturation, 0.0, 1.0, 0.95, 0.62); // lighter when grey
float alpha = interp1d(lin_grad_point, 0.75 / 2., 0.75, 0.4, 0.0); // matches previous alpha fade
bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha));
if (i == valid_points.length() - 1) break;
}
assert(final_idx == (valid_points.length() - 1));
}
} 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);
// double dt = millis_since_boot() - t;
// qDebug() << "Took" << dt << "ms to draw path";
painter.restore();
}
void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) {
const UIScene &scene = s->scene;
painter.save();
// base icon
int x = rightHandDM ? rect().right() - (btn_size - 24) / 2 - (bdr_s * 2) : (btn_size - 24) / 2 + (bdr_s * 2);
int y = rect().bottom() - footer_h / 2;
float opacity = dmActive ? 0.65 : 0.2;
drawIcon(painter, x, y, dm_img, blackColor(0), opacity);
// circle background
painter.setOpacity(1.0);
painter.setPen(Qt::NoPen);
painter.setBrush(blackColor(70));
painter.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size);
// 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 = 10.;
const float leadBuff = 40.;
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}};
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();
const cereal::RadarState::Reader &radar_state = sm["radarState"].getRadarState();
// 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) {
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(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->worldObjectsVisible()) {
if (sm.rcv_frame("modelV2") > s->scene.started_frame) {
update_model(s, sm["modelV2"].getModelV2(), sm["uiPlan"].getUiPlan());
if (sm.rcv_frame("radarState") > s->scene.started_frame) {
update_leads(s, radar_state, sm["modelV2"].getModelV2().getPosition());
}
}
drawLaneLines(painter, s);
if (s->scene.longitudinal_control) {
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 (!hideDM && (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);
}
void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
CameraWidget::showEvent(event);
ui_update_params(uiState());
prev_draw_t = millis_since_boot();
}