diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 80c3f97d7..4da269876 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -220,6 +220,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState } } + if (frogpilot_toggles.value("radar_tracks").toBool()) { + paintRadarTracks(p); + } + if (frogpilot_toggles.value("road_name_ui").toBool()) { paintRoadName(p); } @@ -447,6 +451,31 @@ void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p) p.restore(); } +void FrogPilotAnnotatedCameraWidget::paintRadarTracks(QPainter &p) { + if (radar_tracks.empty()) { + return; + } + + p.save(); + + int diameter = 25; + + float radius = diameter / 2.0f; + float track_x = p.viewport().width() - diameter; + float track_y = p.viewport().height() - diameter; + + p.setBrush(redColor()); + + for (const QPointF &track : radar_tracks) { + float x = std::clamp(static_cast(track.x()), 0.0f, track_x); + float y = std::clamp(static_cast(track.y()), 0.0f, track_y); + + p.drawEllipse(QPointF(x + radius, y + radius), radius, radius); + } + + p.restore(); +} + void FrogPilotAnnotatedCameraWidget::paintRoadName(QPainter &p) { if (roadName.isEmpty()) { return; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index ac89f8915..04acd9c15 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -25,6 +25,8 @@ public: float speed; + std::vector radar_tracks; + FrogPilotUIScene frogpilot_scene; QColor blueColor(int alpha = 255) { return QColor(0, 0, 255, alpha); } @@ -50,6 +52,7 @@ private: void paintCompass(QPainter &p); void paintCurveSpeedControl(QPainter &p); void paintCurveSpeedControlTraining(QPainter &p); + void paintRadarTracks(QPainter &p); void paintRoadName(QPainter &p); void paintStandstillTimer(QPainter &p); void paintStoppingPoint(QPainter &p); diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 3bacd090f..3ba3908d8 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -66,6 +66,9 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { } // FrogPilot variables + if (frogpilot_toggles.value("radar_tracks").toBool()) { + updateRadarTracks(model.getPosition()); + } painter.restore(); } @@ -304,3 +307,24 @@ void ModelRenderer::updateAdjacentLeads(const cereal::FrogPilotRadarState::Reade } } } + +void ModelRenderer::updateRadarTracks(const cereal::XYZTData::Reader &line) { + std::vector &radar_tracks = frogpilot_nvg->radar_tracks; + radar_tracks.clear(); + + SubMaster &fpsm = *(frogpilotUIState()->sm); + capnp::List::Reader radar_points = fpsm["liveTracks"].getLiveTracks().getPoints(); + radar_tracks.reserve(radar_points.size()); + + capnp::List::Reader line_z = line.getZ(); + + for (cereal::RadarData::RadarPoint::Reader point : radar_points) { + float d_rel = point.getDRel(); + float z = line_z[get_path_length_idx(line, d_rel)]; + + QPointF calibrated_point; + if (mapToScreen(d_rel, -point.getYRel(), z + path_offset_z, &calibrated_point)) { + radar_tracks.push_back(calibrated_point); + } + } +} diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index dd371d88a..df0421bce 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -48,6 +48,7 @@ private: // FrogPilot variables void updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); + void updateRadarTracks(const cereal::XYZTData::Reader &line); QPointF adjacent_lead_vertices[2] = {}; };