diff --git a/common/params_keys.h b/common/params_keys.h index 92853f92f..d569faae0 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -122,4 +122,5 @@ inline static std::unordered_map keys = { {"Version", PERSISTENT}, {"dp_device_last_log", CLEAR_ON_ONROAD_TRANSITION}, {"dp_device_reset_conf", CLEAR_ON_MANAGER_START}, + {"dp_ui_radar_tracks", PERSISTENT}, }; diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index 548b02be5..2722865e6 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/selfdrive/ui/qt/offroad/dp_panel.cc @@ -172,6 +172,11 @@ void DPPanel::add_ui_toggles() { QString::fromUtf8("🐉 ") + tr("UI"), "", }, + { + "dp_ui_radar_tracks", + tr("Display Radar Tracks"), + "", + }, }; QWidget *label = nullptr; @@ -183,6 +188,9 @@ void DPPanel::add_ui_toggles() { addItem(label); continue; } + if (param == "dp_ui_radar_tracks" && !vehicle_has_long_ctrl) { + continue; + } has_toggle = true; auto toggle = new ParamControl(param, title, desc, "", this); diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 52902abdc..ec0c0b69f 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -48,6 +48,11 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { } } + if (s->scene.dp_ui_radar_tracks) { + const auto &live_tracks = sm["liveTracks"].getLiveTracks(); + drawLiveTracks(painter, live_tracks, model, surface_rect); + } + painter.restore(); } @@ -186,6 +191,56 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float (1 - t) * start.alphaF() + t * end.alphaF()); } +void ModelRenderer::drawLiveTracks(QPainter &painter, + const cereal::RadarData::Reader &live_tracks, + const cereal::ModelDataV2::Reader &model_data, + const QRect &surface_rect) { + + // Get the model's predicted path for Z-coordinate calculation + const auto& model_path_position = model_data.getPosition(); + + // Set text properties + painter.setPen(Qt::white); + painter.setFont(QFont("Inter", 24, QFont::Bold)); + + // Iterate through each radar point from live_tracks + for (const auto& point : live_tracks.getPoints()) { + float dRel = point.getDRel(); + float yRel = point.getYRel(); + float yvRel = point.getYvRel(); + float vRel = point.getVRel(); + + // Calculate Z-coordinate using the model's path + float z_on_path = path_offset_z; // Default base offset + + // Ensure dRel is non-negative for indexing + if (dRel >= 0) { + z_on_path += model_path_position.getZ()[get_path_length_idx(model_path_position, dRel)]; + } + + QPointF screen_pos; + // mapToScreen projects a point from car space to screen space + if (mapToScreen(dRel, -yRel, z_on_path, &screen_pos)) { // yRel is negated as in update_leads + // Basic drawing: Draw a small circle for the point + painter.setBrush(QColor(255, 0, 0, 200)); // Cyan color for live tracks + painter.drawEllipse(screen_pos, 10, 10); // Draw a small circle of radius 5 + + // Prepare text to display + QString infoText = QString("ID: %1\nd: %2 m\ny: %3 m\ndV: %4 m/s\nyV: %5 m/s") + .arg(point.getTrackId()) + .arg(dRel, 0, 'f', 2) + .arg(yRel, 0, 'f', 2) + .arg(vRel, 0, 'f', 2) + .arg(yvRel, 0, 'f', 2); + + // Draw text near the point + // Adjust text position for better visibility (e.g., slightly offset from the point) + QRectF textRect(screen_pos.x() + 10, screen_pos.y() - 20, 250, 250); // Adjust size as needed + painter.drawText(textRect, Qt::AlignLeft, infoText); + } + } +} + void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect) { const float speedBuff = 10.; diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index 79547e4b8..9fded449a 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -15,6 +15,7 @@ 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 drawLiveTracks(QPainter &painter, const cereal::RadarData::Reader &live_tracks, const cereal::ModelDataV2::Reader &model_data, const QRect &surface_rect); 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); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 79a245a0e..69df6e110 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -65,6 +65,7 @@ static void update_state(UIState *s) { void ui_update_params(UIState *s) { auto params = Params(); s->scene.is_metric = params.getBool("IsMetric"); + s->scene.dp_ui_radar_tracks = params.getBool("dp_ui_radar_tracks"); } void UIState::updateStatus() { @@ -99,6 +100,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", + "liveTracks", }); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index fd2aee771..c1ecf3472 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -60,6 +60,7 @@ typedef struct UIScene { float light_sensor = -1; bool started, ignition, is_metric; uint64_t started_frame; + bool dp_ui_radar_tracks = false; } UIScene; class UIState : public QObject { diff --git a/system/manager/manager.py b/system/manager/manager.py index 5dc08c6d3..02e2c42a2 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,6 +40,7 @@ def manager_init() -> None: ("OpenpilotEnabledToggle", "1"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), ("DisableLogging", "0"), + ("dp_ui_radar_tracks", "0"), ] if params.get_bool("RecordFrontLock"):