Display the radar's track points

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<float>(track.x()), 0.0f, track_x);
+    float y = std::clamp(static_cast<float>(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;

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -25,6 +25,8 @@ public:
 
   float speed;
 
+  std::vector<QPointF> 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);

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<QPointF> &radar_tracks = frogpilot_nvg->radar_tracks;
+  radar_tracks.clear();
+
+  SubMaster &fpsm = *(frogpilotUIState()->sm);
+  capnp::List<cereal::RadarData::RadarPoint>::Reader radar_points = fpsm["liveTracks"].getLiveTracks().getPoints();
+  radar_tracks.reserve(radar_points.size());
+
+  capnp::List<float>::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);
+    }
+  }
+}

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] = {};
 };