Skip to content

Commit

Permalink
encapsulate model functions and variables into ModelState
Browse files Browse the repository at this point in the history
  • Loading branch information
@deanlee
deanlee committed Jul 17, 2024
1 parent 4b79477 commit c9e4002
Show file tree
Hide file tree
Showing 7 changed files with 279 additions and 246 deletions.
2 changes: 1 addition & 1 deletion selfdrive/ui/SConscript
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ qt_src = ["main.cc", "qt/sidebar.cc", "qt/body.cc",
"qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc",
"qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc",
"qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc",
"qt/onroad/buttons.cc", "qt/onroad/alerts.cc"]
"qt/onroad/buttons.cc", "qt/onroad/alerts.cc", 'qt/onroad/model_state.cc']

# build translation files
with open(File("translations/languages.json").abspath) as f:
Expand Down
125 changes: 7 additions & 118 deletions selfdrive/ui/qt/onroad/annotated_camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,78 +134,16 @@ void AnnotatedCameraWidget::initializeGL() {

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)
QTransform car_space_transform;
car_space_transform.translate(width() / 2 - x_offset, height() / 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();

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
QLinearGradient bg(0, height(), 0, 0);
if (sm["controlsState"].getControlsState().getExperimentalMode()) {
// The first half of track_vertices are the points for the right side of the path
const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration().getX();
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration.size());

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();

// 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 {
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);

painter.restore();
model_state.setTransform(width(), height(), car_space_transform, calibration, intrinsic_matrix);
}

void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) {
Expand Down Expand Up @@ -249,42 +187,6 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
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);
Expand Down Expand Up @@ -323,9 +225,9 @@ void AnnotatedCameraWidget::paintGL() {
}
CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);

s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
bool 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;
auto calib = wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;
CameraWidget::updateCalibration(calib);
} else {
CameraWidget::updateCalibration(DEFAULT_CALIBRATION);
Expand All @@ -339,21 +241,8 @@ void AnnotatedCameraWidget::paintGL() {
painter.setPen(Qt::NoPen);

if (s->scene.world_objects_visible) {
update_model(s, model);
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]);
}
}
model_state.update(sm);
model_state.draw(painter, sm);
}

// DMoji
Expand Down
5 changes: 2 additions & 3 deletions selfdrive/ui/qt/onroad/annotated_camera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <memory>

#include "selfdrive/ui/qt/onroad/buttons.h"
#include "selfdrive/ui/qt/onroad/model_state.h"
#include "selfdrive/ui/qt/widgets/cameraview.h"

class AnnotatedCameraWidget : public CameraWidget {
Expand Down Expand Up @@ -40,14 +41,12 @@ class AnnotatedCameraWidget : public CameraWidget {
void initializeGL() override;
void showEvent(QShowEvent *event) override;
void updateFrameMat() override;
void drawLaneLines(QPainter &painter, const UIState *s);
void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd);
void drawHud(QPainter &p);
void drawDriverState(QPainter &painter, const UIState *s);
inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); }
inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); }

double prev_draw_t = 0;
FirstOrderFilter fps_filter;
ModelState model_state;
};
Loading

0 comments on commit c9e4002

Please sign in to comment.