fix lead2 not updated (#21895)

selfdrive/ui/ui.cc

@@ -63,11 +63,15 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
   return max_idx;
 }
 
-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]);
-
+static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) {
+  auto leaders = model.getLeads();
+  auto model_position = model.getPosition();
+  for (int i = 0; i < 2; ++i) {
+    if (leaders[i].getProb() > 0.5) {
+      auto xyva = leaders[i].getXyva();
+      float z = model_position.getZ()[get_path_length_idx(model_position, xyva[0])];
+      calib_frame_to_full_frame(s, xyva[0], xyva[1], z + 1.22, &s->scene.lead_vertices[i]);
+    }
   }
 }
 
@@ -109,7 +113,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
   }
 
   // update path
-  auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
+  auto lead_one = model.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);
@@ -132,11 +136,9 @@ static void update_state(UIState *s) {
     scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
   }
   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["modelV2"].getModelV2().getLeads()[0], line);
+    auto model = sm["modelV2"].getModelV2();
+    update_model(s, model);
+    update_leads(s, model);
   }
   if (sm.updated("liveCalibration")) {
     scene.world_objects_visible = true;
@@ -155,9 +157,6 @@ static void update_state(UIState *s) {
       }
     }
   }
-  if (sm.updated("modelV2") && s->vg) {
-    update_model(s, sm["modelV2"].getModelV2());
-  }
   if (sm.updated("pandaState")) {
     auto pandaState = sm["pandaState"].getPandaState();
     scene.pandaType = pandaState.getPandaType();