mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-23 15:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ui: draw all radard leads (#22901)

Browse Source 
* draw leads detected from radar since it has a low speed override

* comment

* fill lead_verticies

* fix

* Revert changes

Revert "fix"

This reverts commit 7a478c435a0913e09512801309cf52654f3e17ac.

Revert "fill lead_verticies"

This reverts commit 5cdf265bb98480413a4a83c093b3e00489ba2f94.

Revert "comment"

This reverts commit 24a8a3fc2beef263909810d828d4e4fb5ea576e8.

Revert "draw leads detected from radar since it has a low speed override"

This reverts commit 4679fa5d16c7e961def9aaa52e92fb7d994a830d.

* Revert "Draw model leads (#21864)"

This commit reverts 276b00ccb8
This commit is contained in:
sshane
2021-11-17 07:29:54 -06:00
committed by GitHub
parent ba0c844ad9
commit fa55dbe983
2 changed files with 24 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/paint.cc
+7 -7
View File
@@ -64,15 +64,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::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
static void draw_lead(UIState *s, const cereal::RadarState::LeadData::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.getX()[0];
float v_rel = lead_data.getV()[0];
float d_rel = lead_data.getDRel();
float v_rel = lead_data.getVRel();
if (d_rel < leadBuff) {
fillAlpha = 255*(1.0-(d_rel/leadBuff));
if (v_rel < 0) {
@@ -139,12 +139,12 @@ static void ui_draw_world(UIState *s) {
// Draw lead indicators if openpilot is handling longitudinal
if (s->scene.longitudinal_control) {
auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
if (lead_one.getProb() > .5) {
auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
auto lead_two = (*s->sm)["radarState"].getRadarState().getLeadTwo();
if (lead_one.getStatus()) {
draw_lead(s, lead_one, s->scene.lead_vertices[0]);
}
if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
draw_lead(s, lead_two, s->scene.lead_vertices[1]);
}
}
selfdrive/ui/ui.cc
+17 -13
View File
@@ -44,13 +44,12 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
return max_idx;
}
static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) {
auto leads = model.getLeadsV3();
auto model_position = model.getPosition();
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) {
if (leads[i].getProb() > 0.5) {
float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])];
calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[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;
calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
}
}
}
@@ -93,9 +92,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
}
// update path
auto lead_one = model.getLeadsV3()[0];
if (lead_one.getProb() > 0.5) {
const float lead_d = lead_one.getX()[0] * 2.;
auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
if (lead_one.getStatus()) {
const float lead_d = lead_one.getDRel() * 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);
@@ -118,9 +117,14 @@ static void update_state(UIState *s) {
scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
}
if (sm.updated("modelV2") && s->vg) {
auto model = sm["modelV2"].getModelV2();
update_model(s, model);
update_leads(s, model);
update_model(s, sm["modelV2"].getModelV2());
}
if (sm.updated("radarState") && 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);
}
if (sm.updated("liveCalibration")) {
scene.world_objects_visible = true;
@@ -225,7 +229,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", "deviceState", "roadCameraState",
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
});