spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

UI rendering logic, not rotating line yet

stop-lines
mitchellgoffpc 2021-09-14 19:56:21 -07:00
parent ba57a8d1ce
commit 9ffb56c8c9
3 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
selfdrive/ui/paint.cc
View File

@ -64,7 +64,25 @@ 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 ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
if (vd.cnt == 0) return;
const vertex_data *v = &vd.v[0];
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, v[0].x, v[0].y);
for (int i = 1; i < vd.cnt; i++) {
nvgLineTo(s->vg, v[i].x, v[i].y);
}
nvgClosePath(s->vg);
if (color) {
nvgFillColor(s->vg, *color);
} else if (paint) {
nvgFillPaint(s->vg, *paint);
}
nvgFill(s->vg);
}
static void ui_draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
// Draw lead car indicator
auto [x, y] = vd;
@ -87,22 +105,9 @@ static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader
draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
}
static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
if (vd.cnt == 0) return;
const vertex_data *v = &vd.v[0];
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, v[0].x, v[0].y);
for (int i = 1; i < vd.cnt; i++) {
nvgLineTo(s->vg, v[i].x, v[i].y);
}
nvgClosePath(s->vg);
if (color) {
nvgFillColor(s->vg, *color);
} else if (paint) {
nvgFillPaint(s->vg, *paint);
}
nvgFill(s->vg);
static void ui_draw_stop_line(UIState *s, const cereal::ModelDataV2::StopLineData::Reader &stop_line_data, const line_vertices_data &vd) {
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, stop_line_data.getProb());
ui_draw_line(s, vd, &color, nullptr);
}
static void ui_draw_vision_lane_lines(UIState *s) {
@ -137,15 +142,19 @@ static void ui_draw_world(UIState *s) {
// Draw lane edges and vision/mpc tracks
ui_draw_vision_lane_lines(s);
// Draw lead indicators if openpilot is handling longitudinal
// Draw lead and stop line 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];
auto stop_line = (*s->sm)["modelV2"].getModelV2().getStopLine();
if (lead_one.getProb() > .5) {
draw_lead(s, lead_one, s->scene.lead_vertices[0]);
ui_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)) {
draw_lead(s, lead_two, s->scene.lead_vertices[1]);
ui_draw_lead(s, lead_two, s->scene.lead_vertices[1]);
}
if (stop_line.getProb() > .5) {
ui_draw_stop_line(s, stop_line, s->scene.stop_line_vertices);
}
}
nvgResetScissor(s->vg);

18
selfdrive/ui/ui.cc
View File

@ -65,6 +65,18 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa
assert(pvd->cnt <= std::size(pvd->v));
}
static void update_stop_line_data(const UIState *s, const cereal::ModelDataV2::StopLineData::Reader &line,
float x_off, float y_off, float z_off, line_vertices_data *pvd) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
vertex_data *v = &pvd->v[0];
calib_frame_to_full_frame(s, line_x + x_off, line_y - y_off, line_z + z_off, v);
calib_frame_to_full_frame(s, line_x + x_off, line_y, line_z + z_off, v);
calib_frame_to_full_frame(s, line_x + x_off, line_y + y_off, line_z + z_off, v);
calib_frame_to_full_frame(s, line_x - x_off, line_y + y_off, line_z + z_off, v);
calib_frame_to_full_frame(s, line_x - x_off, line_y, line_z + z_off, v);
calib_frame_to_full_frame(s, line_x - x_off, line_y - y_off, line_z + z_off, v);
}
static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
UIScene &scene = s->scene;
auto model_position = model.getPosition();
@ -96,6 +108,12 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
}
max_idx = get_path_length_idx(model_position, max_distance);
update_line_data(s, model_position, 0.5, 1.22, &scene.track_vertices, max_idx);
// update stop lines
const auto stop_line = model.getStopLine();
if (stop_line.getProb() > .5) {
update_stop_line_data(s, stop_line, 2, .05, 1.22, &scene.stop_line_vertices);
}
}
static void update_sockets(UIState *s) {

3
selfdrive/ui/ui.h
View File

@ -55,7 +55,7 @@ typedef struct Alert {
}
} Alert;
const Alert CONTROLS_WAITING_ALERT = {"openpilot Unavailable", "Waiting for controls to start",
const Alert CONTROLS_WAITING_ALERT = {"openpilot Unavailable", "Waiting for controls to start",
"controlsWaiting", cereal::ControlsState::AlertSize::MID,
AudibleAlert::NONE};
@ -106,6 +106,7 @@ typedef struct UIScene {
line_vertices_data track_vertices;
line_vertices_data lane_line_vertices[4];
line_vertices_data road_edge_vertices[2];
line_vertices_data stop_line_vertices;
bool dm_active, engageable;