From 276b00ccb805b8071db4352b406f39233c2c3867 Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Thu, 5 Aug 2021 20:35:03 -0700
Subject: [PATCH] Draw model leads (#21864)

* draw model not radar

* compiles

* wrong way
---
 selfdrive/ui/paint.cc | 15 +++++++--------
 selfdrive/ui/ui.cc    | 25 +++++++++++--------------
 2 files changed, 18 insertions(+), 22 deletions(-)

diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index ac944fd872d090..ca0b668531d916 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -67,15 +67,15 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
   ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
 }
 
-static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead_data, const vertex_data &vd) {
+static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, const vertex_data &vd) {
   // Draw lead car indicator
   auto [x, y] = vd;
 
   float fillAlpha = 0;
   float speedBuff = 10.;
   float leadBuff = 40.;
-  float d_rel = lead_data.getDRel();
-  float v_rel = lead_data.getVRel();
+  float d_rel = lead_data.getXyva()[0];
+  float v_rel = lead_data.getXyva()[2];
   if (d_rel < leadBuff) {
     fillAlpha = 255*(1.0-(d_rel/leadBuff));
     if (v_rel < 0) {
@@ -167,13 +167,12 @@ static void ui_draw_world(UIState *s) {
 
   // Draw lead indicators if openpilot is handling longitudinal
   if (s->scene.longitudinal_control) {
-    auto radar_state = (*s->sm)["radarState"].getRadarState();
-    auto lead_one = radar_state.getLeadOne();
-    auto lead_two = radar_state.getLeadTwo();
-    if (lead_one.getStatus()) {
+    auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
+    auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeads()[1];
+    if (lead_one.getProb() > .5) {
       draw_lead(s, lead_one, s->scene.lead_vertices[0]);
     }
-    if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
+   if (lead_two.getProb() > .5 && (std::abs(lead_one.getXyva()[0] - lead_two.getXyva()[0]) > 3.0)) {
       draw_lead(s, lead_two, s->scene.lead_vertices[1]);
     }
   }
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 8c1477b23fb265..a7110873dbfd16 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -63,14 +63,11 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
   return max_idx;
 }
 
-static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
-  for (int i = 0; i < 2; ++i) {
-    auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
-    if (lead_data.getStatus()) {
-      float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getDRel())] : 0.0;
-      // negative because radarState uses left positive convention
-      calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
-    }
+static void update_leads(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
+  if (lead_data.getProb() > 0.5) {
+    float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getXyva()[0])] : 0.0;
+    calib_frame_to_full_frame(s, lead_data.getXyva()[0], lead_data.getXyva()[1], z + 1.22, &s->scene.lead_vertices[0]);
+
   }
 }
 
@@ -112,9 +109,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
   }
 
   // update path
-  auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
-  if (lead_one.getStatus()) {
-    const float lead_d = lead_one.getDRel() * 2.;
+  auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
+  if (lead_one.getProb() > 0.5) {
+    const float lead_d = lead_one.getXyva()[0] * 2.;
     max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
   }
   max_idx = get_path_length_idx(model_position, max_distance);
@@ -134,12 +131,12 @@ static void update_state(UIState *s) {
     scene.engageable = sm["controlsState"].getControlsState().getEngageable();
     scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
   }
-  if (sm.updated("radarState") && s->vg) {
+  if (sm.updated("modelV2") && s->vg) {
     std::optional<cereal::ModelDataV2::XYZTData::Reader> line;
     if (sm.rcv_frame("modelV2") > 0) {
       line = sm["modelV2"].getModelV2().getPosition();
     }
-    update_leads(s, sm["radarState"].getRadarState(), line);
+    update_leads(s, sm["modelV2"].getModelV2().getLeads()[0], line);
   }
   if (sm.updated("liveCalibration")) {
     scene.world_objects_visible = true;
@@ -273,7 +270,7 @@ static void update_status(UIState *s) {
 
 QUIState::QUIState(QObject *parent) : QObject(parent) {
   ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
-    "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
+    "modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
     "pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
   });