From 2de6ab5b68bc528e913db68f7fe576b6ebe89a9b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 13 Jul 2023 02:54:23 -0700 Subject: [PATCH] add a bunch of debugging stuff i don't want to lose --- selfdrive/ui/qt/onroad.cc | 26 +++++++++++++++-- selfdrive/ui/qt/onroad.h | 2 ++ selfdrive/ui/qt/util.cc | 4 +++ selfdrive/ui/ui.cc | 59 ++++++++++++++++++++++++++++----------- selfdrive/ui/ui.h | 2 +- 5 files changed, 73 insertions(+), 20 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ef711afd50..e889bea07b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -443,8 +443,22 @@ 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.drawPolygon(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); } // road edges @@ -663,6 +677,14 @@ 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 955ddf254d..2fec0a41a5 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -72,6 +72,8 @@ 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 72bc07f27f..7127268f3a 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -9,6 +9,7 @@ #include #include #include +#include #include #include "common/params.h" @@ -96,6 +97,9 @@ 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 6314ef6ba0..388f2e3bca 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -69,7 +69,10 @@ 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 - if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) { + 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) { continue; } left_points.push_back(left); @@ -79,16 +82,38 @@ 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(); @@ -99,22 +124,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 2519f8ea2c..d74cfa4f3f 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[4]; + QPolygonF lane_line_vertices[2]; QPolygonF road_edge_vertices[2]; // lead