diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index da652f7bfd..643951fff3 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -30,7 +30,7 @@ qt_src = ["main.cc", "ui.cc", "qt/sidebar.cc", "qt/body.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.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/onroad_home.cc", "qt/onroad/annotated_camera.cc", "qt/onroad/model.cc", "qt/onroad/buttons.cc", "qt/onroad/alerts.cc", "qt/onroad/driver_monitoring.cc", "qt/onroad/hud.cc"] # build translation files diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index fd8ad19c2e..f504ad69f1 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -75,8 +75,7 @@ mat4 AnnotatedCameraWidget::calcFrameMatrix() { 0.0f, zoom, (h / 2 - y_offset) - (center_y * zoom), 0.0f, 0.0f, 1.0f).finished(); - s->car_space_transform = video_transform * calib_transform; - s->clip_region = rect().adjusted(-500, -500, 500, 500); + model.setTransform(video_transform * calib_transform); float zx = zoom * 2 * center_x / w; float zy = zoom * 2 * center_y / h; @@ -88,106 +87,10 @@ mat4 AnnotatedCameraWidget::calcFrameMatrix() { }}; } -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(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(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["selfdriveState"].getSelfdriveState().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(scene.track_vertices.length() / 2, acceleration.size()); - - for (int i = 0; i < max_len; ++i) { - // Some points are out of frame - int track_idx = max_len - i - 1; // flip idx to start from bottom right - if (scene.track_vertices[track_idx].y() < 0 || scene.track_vertices[track_idx].y() > height()) continue; - - // Flip so 0 is bottom of frame - float lin_grad_point = (height() - scene.track_vertices[track_idx].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(); -} - -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(); // draw camera frame { @@ -218,7 +121,7 @@ void AnnotatedCameraWidget::paintGL() { wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); } CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); - CameraWidget::setFrameId(model.getFrameId()); + CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId()); CameraWidget::paintGL(); } @@ -226,24 +129,7 @@ void AnnotatedCameraWidget::paintGL() { painter.setRenderHint(QPainter::Antialiasing); 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.draw(painter, rect()); dmon.draw(painter, rect()); hud.updateState(*s); hud.draw(painter, rect()); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 324a6d7458..d205579f6c 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -5,6 +5,7 @@ #include "selfdrive/ui/qt/onroad/hud.h" #include "selfdrive/ui/qt/onroad/buttons.h" #include "selfdrive/ui/qt/onroad/driver_monitoring.h" +#include "selfdrive/ui/qt/onroad/model.h" #include "selfdrive/ui/qt/widgets/cameraview.h" class AnnotatedCameraWidget : public CameraWidget { @@ -19,6 +20,7 @@ private: ExperimentalButton *experimental_btn; DriverMonitorRenderer dmon; HudRenderer hud; + ModelRenderer model; std::unique_ptr pm; int skip_frame_count = 0; @@ -29,9 +31,6 @@ protected: void initializeGL() override; void showEvent(QShowEvent *event) override; mat4 calcFrameMatrix() override; - void drawLaneLines(QPainter &painter, const UIState *s); - void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); - inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } double prev_draw_t = 0; FirstOrderFilter fps_filter; diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc new file mode 100644 index 0000000000..9a2c989b26 --- /dev/null +++ b/selfdrive/ui/qt/onroad/model.cc @@ -0,0 +1,208 @@ +#include "selfdrive/ui/qt/onroad/model.h" + +constexpr int CLIP_MARGIN = 500; +constexpr float MIN_DRAW_DISTANCE = 10.0; +constexpr float MAX_DRAW_DISTANCE = 100.0; + +static int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { + const auto &line_x = line.getX(); + int max_idx = 0; + for (int i = 1; i < line_x.size() && line_x[i] <= path_height; ++i) { + max_idx = i; + } + return max_idx; +} + +void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { + auto &sm = *(uiState()->sm); + if (sm.updated("carParams")) { + longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); + } + + // Check if data is up-to-date + if (!(sm.alive("liveCalibration") && sm.alive("modelV2"))) { + return; + } + + clip_region = surface_rect.adjusted(-CLIP_MARGIN, -CLIP_MARGIN, CLIP_MARGIN, CLIP_MARGIN); + experimental_model = sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); + + painter.save(); + + const auto &model = sm["modelV2"].getModelV2(); + const auto &radar_state = sm["radarState"].getRadarState(); + const auto &lead_one = radar_state.getLeadOne(); + + update_model(model, lead_one); + drawLaneLines(painter); + drawPath(painter, model, surface_rect.height()); + + if (longitudinal_control && sm.alive("radarState")) { + update_leads(radar_state, model.getPosition()); + const auto &lead_two = radar_state.getLeadTwo(); + if (lead_one.getStatus()) { + drawLead(painter, lead_one, lead_vertices[0], surface_rect); + } + if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { + drawLead(painter, lead_two, lead_vertices[1], surface_rect); + } + } + + painter.restore(); +} + +void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) { + for (int i = 0; i < 2; ++i) { + const auto &lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); + if (lead_data.getStatus()) { + float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; + mapToScreen(lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &lead_vertices[i]); + } + } +} + +void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) { + const auto &model_position = model.getPosition(); + float max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); + + // update lane lines + const auto &lane_lines = model.getLaneLines(); + const auto &line_probs = model.getLaneLineProbs(); + int max_idx = get_path_length_idx(lane_lines[0], max_distance); + for (int i = 0; i < std::size(lane_line_vertices); i++) { + lane_line_probs[i] = line_probs[i]; + mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx); + } + + // update road edges + const auto &road_edges = model.getRoadEdges(); + const auto &edge_stds = model.getRoadEdgeStds(); + for (int i = 0; i < std::size(road_edge_vertices); i++) { + road_edge_stds[i] = edge_stds[i]; + mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx); + } + + // update path + if (lead.getStatus()) { + const float lead_d = lead.getDRel() * 2.; + max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); + } + max_idx = get_path_length_idx(model_position, max_distance); + mapLineToPolygon(model_position, 0.9, 1.22, &track_vertices, max_idx, false); +} + +void ModelRenderer::drawLaneLines(QPainter &painter) { + // lanelines + for (int i = 0; i < std::size(lane_line_vertices); ++i) { + painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(lane_line_probs[i], 0.0, 0.7))); + painter.drawPolygon(lane_line_vertices[i]); + } + + // road edges + for (int i = 0; i < std::size(road_edge_vertices); ++i) { + painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp(1.0 - road_edge_stds[i], 0.0, 1.0))); + painter.drawPolygon(road_edge_vertices[i]); + } +} + +void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) { + QLinearGradient bg(0, height, 0, 0); + if (experimental_model) { + // The first half of track_vertices are the points for the right side of the path + const auto &acceleration = model.getAcceleration().getX(); + const int max_len = std::min(track_vertices.length() / 2, acceleration.size()); + + for (int i = 0; i < max_len; ++i) { + // Some points are out of frame + int track_idx = max_len - i - 1; // flip idx to start from bottom right + if (track_vertices[track_idx].y() < 0 || track_vertices[track_idx].y() > height) continue; + + // Flip so 0 is bottom of frame + float lin_grad_point = (height - track_vertices[track_idx].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(track_vertices); +} + +void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, + const QPointF &vd, const QRect &surface_rect) { + 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(vd.x(), 0.f, surface_rect.width() - sz / 2); + float y = std::min(vd.y(), surface_rect.height() - sz * 0.6); + + 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(QColor(201, 34, 49, fillAlpha)); + painter.drawPolygon(chevron, std::size(chevron)); +} + +// Projects a point in car to space to the corresponding point in full frame image space. +bool ModelRenderer::mapToScreen(float in_x, float in_y, float in_z, QPointF *out) { + Eigen::Vector3f input(in_x, in_y, in_z); + auto pt = car_space_transform * input; + *out = QPointF(pt.x() / pt.z(), pt.y() / pt.z()); + return clip_region.contains(*out); +} + +void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off, + QPolygonF *pvd, int max_idx, bool allow_invert) { + const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); + QPointF left, right; + pvd->clear(); + for (int i = 0; i <= max_idx; i++) { + // highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera + if (line_x[i] < 0) continue; + + bool l = mapToScreen(line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left); + bool r = mapToScreen(line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right); + if (l && r) { + // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts + if (!allow_invert && pvd->size() && left.y() > pvd->back().y()) { + continue; + } + pvd->push_back(left); + pvd->push_front(right); + } + } +} diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h new file mode 100644 index 0000000000..358fce53f4 --- /dev/null +++ b/selfdrive/ui/qt/onroad/model.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "selfdrive/ui/ui.h" + +class ModelRenderer { +public: + ModelRenderer() {} + void setTransform(const Eigen::Matrix3f &transform) { car_space_transform = transform; } + void draw(QPainter &painter, const QRect &surface_rect); + +private: + bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out); + void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off, + QPolygonF *pvd, int max_idx, bool allow_invert = true); + void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect); + void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); + void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); + void drawLaneLines(QPainter &painter); + void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height); + + bool longitudinal_control = false; + bool experimental_model = false; + float lane_line_probs[4] = {}; + float road_edge_stds[2] = {}; + QPolygonF track_vertices; + QPolygonF lane_line_vertices[4] = {}; + QPolygonF road_edge_vertices[2] = {}; + QPointF lead_vertices[2] = {}; + Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero(); + QRectF clip_region; +}; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 852980d54e..a9206b59bb 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -14,90 +14,6 @@ #define BACKLIGHT_DT 0.05 #define BACKLIGHT_TS 10.00 -// Projects a point in car to space to the corresponding point in full frame -// image space. -static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, QPointF *out) { - Eigen::Vector3f input(in_x, in_y, in_z); - auto transformed = s->car_space_transform * input; - *out = QPointF(transformed.x() / transformed.z(), transformed.y() / transformed.z()); - return s->clip_region.contains(*out); -} - -int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { - const auto line_x = line.getX(); - int max_idx = 0; - for (int i = 1; i < line_x.size() && line_x[i] <= path_height; ++i) { - max_idx = i; - } - return max_idx; -} - -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) { - for (int i = 0; i < 2; ++i) { - auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); - if (lead_data.getStatus()) { - float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; - calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]); - } - } -} - -void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, - float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { - const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); - QPointF left, right; - pvd->clear(); - for (int i = 0; i <= max_idx; i++) { - // highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera - if (line_x[i] < 0) continue; - - bool l = calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left); - bool r = calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right); - if (l && r) { - // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts - if (!allow_invert && pvd->size() && left.y() > pvd->back().y()) { - continue; - } - pvd->push_back(left); - pvd->push_front(right); - } - } -} - -void update_model(UIState *s, - const cereal::ModelDataV2::Reader &model) { - UIScene &scene = s->scene; - auto model_position = model.getPosition(); - float max_distance = std::clamp(*(model_position.getX().end() - 1), - MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); - - // update lane lines - const auto lane_lines = model.getLaneLines(); - const auto lane_line_probs = model.getLaneLineProbs(); - int max_idx = get_path_length_idx(lane_lines[0], max_distance); - for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { - scene.lane_line_probs[i] = lane_line_probs[i]; - update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_idx); - } - - // update road edges - const auto road_edges = model.getRoadEdges(); - const auto road_edge_stds = model.getRoadEdgeStds(); - for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { - scene.road_edge_stds[i] = road_edge_stds[i]; - update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx); - } - - // update path - auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne(); - if (lead_one.getStatus()) { - const float lead_d = lead_one.getDRel() * 2.; - max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); - } - max_idx = get_path_length_idx(model_position, max_distance); - update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); -} - static void update_sockets(UIState *s) { s->sm->update(0); } @@ -136,9 +52,6 @@ static void update_state(UIState *s) { } else if ((s->sm->frame - s->sm->rcv_frame("pandaStates")) > 5*UI_FREQ) { scene.pandaType = cereal::PandaState::PandaType::UNKNOWN; } - if (sm.updated("carParams")) { - scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); - } if (sm.updated("wideRoadCameraState")) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; @@ -147,11 +60,6 @@ static void update_state(UIState *s) { scene.light_sensor = -1; } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; - - scene.world_objects_visible = scene.world_objects_visible || - (scene.started && - sm.rcv_frame("liveCalibration") > scene.started_frame && - sm.rcv_frame("modelV2") > scene.started_frame); } void ui_update_params(UIState *s) { @@ -177,7 +85,6 @@ void UIState::updateStatus() { scene.started_frame = sm->frame; } started_prev = scene.started; - scene.world_objects_visible = false; emit offroadTransition(!scene.started); } } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 8a06a4cccb..6ff67cacde 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -7,7 +7,6 @@ #include #include #include -#include #include "cereal/messaging/messaging.h" #include "common/mat.h" @@ -22,8 +21,6 @@ const int UI_HEADER_HEIGHT = 420; const int UI_FREQ = 20; // Hz const int BACKLIGHT_OFFROAD = 50; -const float MIN_DRAW_DISTANCE = 10.0; -const float MAX_DRAW_DISTANCE = 100.0; const Eigen::Matrix3f VIEW_FROM_DEVICE = (Eigen::Matrix3f() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, @@ -58,21 +55,10 @@ typedef struct UIScene { Eigen::Matrix3f view_from_wide_calib = VIEW_FROM_DEVICE; cereal::PandaState::PandaType pandaType; - // modelV2 - float lane_line_probs[4]; - float road_edge_stds[2]; - QPolygonF track_vertices; - QPolygonF lane_line_vertices[4]; - QPolygonF road_edge_vertices[2]; - - // lead - QPointF lead_vertices[2]; - cereal::LongitudinalPersonality personality; float light_sensor = -1; - bool started, ignition, is_metric, longitudinal_control; - bool world_objects_visible = false; + bool started, ignition, is_metric; uint64_t started_frame; } UIScene; @@ -89,13 +75,9 @@ public: std::unique_ptr sm; UIStatus status; UIScene scene = {}; - QString language; PrimeState *prime_state; - Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero(); - QRectF clip_region; - signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); @@ -145,11 +127,4 @@ public slots: }; Device *device(); - void ui_update_params(UIState *s); -int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height); -void update_model(UIState *s, - const cereal::ModelDataV2::Reader &model); -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); -void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, - float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert);