draw lead indicator in 3D (#19793)

* lead indicator in 3D

* more

* Update selfdrive/ui/ui.cc

Co-authored-by: Willem Melching <willem.melching@gmail.com>

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/ui/paint.cc

@@ -63,16 +63,7 @@ static void ui_draw_text(const UIState *s, float x, float y, const char* string,
   nvgText(s->vg, x, y, string, NULL);
 }
 
-static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
-                          NVGcolor fillColor, NVGcolor glowColor) {
-  vertex_data out = {};
-  car_space_to_full_frame(s, x_in, y_in, 0.0, &out);
-
-  auto [x, y] = out;
-  sz = std::clamp((sz * 30) / (x_in / 3 + 30), 15.0f, 30.0f) * zoom;
-  y = std::fmin(s->scene.viz_rect.bottom() - sz * .6,  y);
-  x = std::clamp(x, 0.f, s->scene.viz_rect.right() - sz / 2);
-
+static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillColor, NVGcolor glowColor) {
   // glow
   float g_xo = sz/5;
   float g_yo = sz/10;
@@ -109,8 +100,11 @@ static void ui_draw_circle_image(const UIState *s, int x, int y, int size, const
   ui_draw_circle_image(s, x, y, size, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
 }
 
-static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead){
+static void draw_lead(UIState *s, int idx){
   // Draw lead car indicator
+  const auto &lead = s->scene.lead_data[idx];
+  auto [x, y] = s->scene.lead_vertices[idx];
+
   float fillAlpha = 0;
   float speedBuff = 10.;
   float leadBuff = 40.;
@@ -123,7 +117,11 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le
     }
     fillAlpha = (int)(fmin(fillAlpha, 255));
   }
-  draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
+
+  float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * zoom;
+  x = std::clamp(x, 0.f, s->scene.viz_rect.right() - sz / 2);
+  y = std::fmin(s->scene.viz_rect.bottom() - sz * .6,  y);
+  draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
 }
 
 static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) {
@@ -206,10 +204,10 @@ static void ui_draw_world(UIState *s) {
   // Draw lead indicators if openpilot is handling longitudinal
   if (s->longitudinal_control) {
     if (scene->lead_data[0].getStatus()) {
-      draw_lead(s, scene->lead_data[0]);
+      draw_lead(s, 0);
     }
     if (scene->lead_data[1].getStatus() && (std::abs(scene->lead_data[0].getDRel() - scene->lead_data[1].getDRel()) > 3.0)) {
-      draw_lead(s, scene->lead_data[1]);
+      draw_lead(s, 1);
     }
   }
   nvgResetScissor(s->vg);

selfdrive/ui/ui.cc

@@ -56,6 +56,25 @@ void ui_init(UIState *s) {
   s->vipc_client = s->vipc_client_rear;
 }
 
+static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) {
+  const auto line_x = line.getX();
+  int max_idx = 0;
+  for (int i = 0; i < TRAJECTORY_SIZE && line_x[i] < path_height; ++i) {
+    max_idx = i;
+  }
+  return max_idx;
+}
+
+static void update_lead(UIState *s, const cereal::RadarState::Reader &radar_state,
+                        const cereal::ModelDataV2::XYZTData::Reader &line, int idx) {
+  auto &lead_data = s->scene.lead_data[idx];
+  lead_data = (idx == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
+  if (lead_data.getStatus()) {
+    const int path_idx = get_path_length_idx(line, lead_data.getDRel());
+    car_space_to_full_frame(s, lead_data.getDRel(), lead_data.getYRel(), -line.getZ()[path_idx], &s->scene.lead_vertices[idx]);
+  }
+}
+
 template <class T>
 static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
                              float y_off, float z_off, T *pvd, float max_distance) {
@@ -113,9 +132,10 @@ static void update_sockets(UIState *s) {
     scene.controls_state = sm["controlsState"].getControlsState();
   }
   if (sm.updated("radarState")) {
-    auto data = sm["radarState"].getRadarState();
-    scene.lead_data[0] = data.getLeadOne();
-    scene.lead_data[1] = data.getLeadTwo();
+    auto radar_state = sm["radarState"].getRadarState();
+    const auto line = sm["modelV2"].getModelV2().getPosition();
+    update_lead(s, radar_state, line, 0);
+    update_lead(s, radar_state, line, 1);
   }
   if (sm.updated("liveCalibration")) {
     scene.world_objects_visible = true;

selfdrive/ui/ui.hpp

@@ -135,6 +135,9 @@ typedef struct UIScene {
   track_vertices_data track_vertices;
   line_vertices_data lane_line_vertices[4];
   line_vertices_data road_edge_vertices[2];
+
+  // lead
+  vertex_data lead_vertices[2];
 } UIScene;
 
 typedef struct UIState {