diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a7110873d..6d570acec 100644 --- a/selfdrive/ui/ui.cc +++ b/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 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 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();