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

draw lead indicator in 3D (#19793)

Browse Source 
* 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>
This commit is contained in:
Dean Lee
2021-01-28 22:53:24 +08:00
committed by GitHub
parent 5693773ae1
commit 6bd9e5ca6a
3 changed files with 38 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/paint.cc
+12 -14
View File
@@ -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
+23 -3
View File
@@ -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
+3
View File
@@ -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 {