diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e889bea07b..ef711afd50 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -443,22 +443,8 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // 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.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, 1.0)); - QPolygonF newp; - if (badp.isEmpty()) { - newp = QPolygonF(scene.lane_line_vertices[i]); - } else { - newp = QPolygonF(badp); -// newp.setPoint(0, newp.point(newp.size() - 1)); -// newp[0] = newp[(newp.size() - 1)]; - qDebug() << "using bad poly"; - } - qDebug() << "size:" << newp.size(); - qDebug() << "is closed:" << newp.isClosed() << "intersected:" << (newp.intersected(newp).size() > 0); - qDebug() << newp; - painter.drawPolygon(newp.united(newp)); -// painter.drawConvexPolygon(newp); + 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 @@ -677,14 +663,6 @@ void AnnotatedCameraWidget::paintGL() { } prev_draw_t = cur_draw_t; - bool high_d_time = false;//(cur_draw_t - start_draw_t) > 45; - - qDebug() << "draw time:" << (cur_draw_t - start_draw_t) << "ms" << (high_d_time ? "high!" : "") << "\n"; - - if (high_d_time && std::size(s->scene.lane_line_vertices) == 1 && badp.size() == 0) { - badp = QPolygonF(s->scene.lane_line_vertices[0]); - } - // publish debug msg MessageBuilder msg; auto m = msg.initEvent().initUiDebug(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 2fec0a41a5..955ddf254d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -72,8 +72,6 @@ private: void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - QPolygonF badp; - ExperimentalButton *experimental_btn; QPixmap dm_img; float speed; diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 7127268f3a..72bc07f27f 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -9,7 +9,6 @@ #include #include #include -#include #include #include "common/params.h" @@ -97,9 +96,6 @@ void setQtSurfaceFormat() { fmt.setRenderableType(QSurfaceFormat::OpenGLES); #endif fmt.setSamples(16); -// fmt.setStencilBufferSize(1); - qDebug() << "depth" << fmt.depthBufferSize() << fmt.stencilBufferSize(); -// fmt.setDepthBufferSize(1); QSurfaceFormat::setDefaultFormat(fmt); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 388f2e3bca..6314ef6ba0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -69,10 +69,7 @@ void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, 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 - bool new_point_higher = left_points.size() && left.y() > left_points.back().y(); - qDebug() << "abs" << (left.y() - left_points.back().y()); -// bool new_point_x_too_close = left_points.size() && std::abs(left.y() - left_points.back().y()) < 0.01; - if (!allow_invert && new_point_higher) { + if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) { continue; } left_points.push_back(left); @@ -82,38 +79,16 @@ void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, *pvd = left_points + right_points; } -QPolygonF simplifyPolygon(const QPolygonF& polygon, qreal minDistance) { - QPolygonF simplifiedPolygon; - - for (int i = 0; i < polygon.size(); ++i) { - const QPointF& current = polygon[i]; - const QPointF& next = polygon[(i + 1) % polygon.size()]; - - if (QLineF(current, next).length() >= minDistance) { - simplifiedPolygon.append(current); - } - } - - return simplifiedPolygon; -} - void update_model(UIState *s, const cereal::ModelDataV2::Reader &model, const cereal::UiPlan::Reader &plan) { UIScene &scene = s->scene; - - auto model_position = model.getPosition(); - float max_distance_orig = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1], - MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); - auto plan_position = plan.getPosition(); if (plan_position.getX().size() < TRAJECTORY_SIZE){ plan_position = model.getPosition(); } float max_distance = std::clamp(plan_position.getX()[TRAJECTORY_SIZE - 1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE); - max_distance = 100; - qDebug() << "max_distance orig, new:" << max_distance_orig << max_distance; // update lane lines const auto lane_lines = model.getLaneLines(); @@ -124,22 +99,22 @@ void update_model(UIState *s, update_line_data(s, lane_lines[i], 0.025, 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(plan_position, max_distance); -// update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); + // 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(plan_position, max_distance); + update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); } void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index d74cfa4f3f..2519f8ea2c 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -118,7 +118,7 @@ typedef struct UIScene { float lane_line_probs[4]; float road_edge_stds[2]; QPolygonF track_vertices; - QPolygonF lane_line_vertices[2]; + QPolygonF lane_line_vertices[4]; QPolygonF road_edge_vertices[2]; // lead