From 1cc43f8d7d203b8c760b346667a9edccb0eb7b8d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 6 Jan 2021 11:34:52 +0100 Subject: [PATCH] UI: refactor model related functions (#19657) Squashed commit of the following: commit 7d7fb15e4cdd0e47fa38f949f79581dc611db015 Author: Willem Melching Date: Wed Jan 6 11:31:09 2021 +0100 no extern commit 901893966e3f908435bceb8777253dfa916a3d91 Merge: 9816e01e c0703a57 Author: Willem Melching Date: Wed Jan 6 11:27:29 2021 +0100 Merge branch 'master' into refactor_model_draw commit 9816e01e6ff0284521fa18b07ec15b3900577687 Author: deanlee Date: Wed Jan 6 04:02:08 2021 +0000 fix lead_d commit 6fa500dbce26544cc3fdeb4d9b0d2d654382a40b Author: deanlee Date: Tue Jan 5 13:18:20 2021 +0000 populate model data in update_socket commit e25db7e28c1b543a04c3e837aa44431c42dbb5b2 Author: deanlee Date: Tue Jan 5 12:59:13 2021 +0000 update model in function commit 0834a3ffe7eaa080671f13c32dd6475ae5392d5b Author: deanlee Date: Tue Jan 5 12:50:21 2021 +0000 continue commit 95b08679c2593eb0770a68b3cf1a59f1fb3b74f4 Author: Willem Melching Date: Tue Jan 5 13:26:34 2021 +0100 Update selfdrive/ui/paint.cc commit 0eef065ae4f96c2ee5d97b4ccdfdbf0ae4945385 Author: Dean Lee Date: Tue Jan 5 20:11:01 2021 +0800 Update selfdrive/ui/paint.cc Co-authored-by: Willem Melching commit 955ad9b03a176174b86b28d5bb6f48d6e2a09281 Author: deanlee Date: Thu Dec 31 04:45:31 2020 +0000 refactor model related functions old-commit-hash: 37cb4bec631197f6b369bf89f94786772bd05336 --- selfdrive/ui/paint.cc | 76 ++++++------------------------------------ selfdrive/ui/paint.hpp | 1 + selfdrive/ui/ui.cc | 63 ++++++++++++++++++++++++---------- selfdrive/ui/ui.hpp | 48 +++++++++----------------- 4 files changed, 74 insertions(+), 114 deletions(-) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index d1a3bd261..4dd7186f4 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -41,7 +41,7 @@ const mat3 intrinsic_matrix = (mat3){{ // Projects a point in car to space to the corresponding point in full frame // image space. -bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0) { +bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin) { const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}}; // We'll call the car space point p. // First project into normalized image coordinates with the extrinsics matrix. @@ -146,35 +146,6 @@ static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcol nvgFill(s->vg); } -static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) { - const UIScene *scene = &s->scene; - const float off = 0.5; - int max_idx = -1; - const auto lead = scene->lead_data[0]; - float lead_d = lead.getStatus() ? lead.getDRel()*2. : MAX_DRAW_DISTANCE; - - float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE; - path_length = fmin(path_length, scene->max_distance); - - - vertex_data *v = &pvd->v[0]; - const float margin = 500.0f; - for (int i = 0; i < TRAJECTORY_SIZE and line.getX()[i] <= path_length; i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y, margin); - max_idx = i; - } - for (int i = max_idx; i >= 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y, margin); - } - pvd->cnt = v - pvd->v; -} - -static void ui_draw_track(UIState *s, track_vertices_data *pvd) { - NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, - COLOR_WHITE, COLOR_WHITE_ALPHA(0)); - ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg); -} - static void draw_frame(UIState *s) { mat4 *out_mat; if (s->scene.frontview) { @@ -206,49 +177,24 @@ static void draw_frame(UIState *s) { glBindVertexArray(0); } -static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) { - // TODO check that this doesn't overflow max vertex buffer - int max_idx = -1; - vertex_data *v = &pvd->v[0]; - const float margin = 500.0f; - for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); - max_idx = i; - } - for (int i = max_idx; i >= 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); - } - pvd->cnt = v - pvd->v; -} - static void ui_draw_vision_lane_lines(UIState *s) { - const UIScene *scene = &s->scene; - + const UIScene &scene = s->scene; // paint lanelines - line_vertices_data *pvd_ll = &s->lane_line_vertices[0]; - for (int ll_idx = 0; ll_idx < 4; ll_idx++) { - if (s->sm->updated("modelV2")) { - update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance); - } - NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]); - ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]); + ui_draw_line(s, scene.lane_line_vertices[i].v, scene.lane_line_vertices[i].cnt, &color, nullptr); } // paint road edges - line_vertices_data *pvd_re = &s->road_edge_vertices[0]; - for (int re_idx = 0; re_idx < 2; re_idx++) { - if (s->sm->updated("modelV2")) { - update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance); - } - NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0)); - ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0)); + ui_draw_line(s, scene.road_edge_vertices[i].v, scene.road_edge_vertices[i].cnt, &color, nullptr); } // paint path - if (s->sm->updated("modelV2")) { - update_track_data(s, scene->model.getPosition(), &s->track_vertices); - } - ui_draw_track(s, &s->track_vertices); + NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + ui_draw_line(s, scene.track_vertices.v, scene.track_vertices.cnt, nullptr, &track_bg); } // Draw all world space objects. diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.hpp index d84bfb0e3..f3b6d1a01 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.hpp @@ -1,6 +1,7 @@ #pragma once #include "ui.hpp" +bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0); void ui_draw(UIState *s); void ui_draw_image(NVGcontext *vg, const Rect &r, int image, float alpha); void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r = 0, int width = 0); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 5b3ded0ef..ee75f8ed0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -16,6 +16,7 @@ #include "ui.hpp" #include "paint.hpp" + int write_param_float(float param, const char* param_name, bool persistent_param) { char s[16]; int size = snprintf(s, sizeof(s), "%f", param); @@ -104,6 +105,50 @@ destroy: s->vision_connected = false; } +template +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) { + const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); + int max_idx = -1; + vertex_data *v = &pvd->v[0]; + const float margin = 500.0f; + for (int i = 0; ((i < TRAJECTORY_SIZE) and (line_x[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { + v += car_space_to_full_frame(s, line_x[i], -line_y[i] - y_off, -line_z[i] + z_off, &v->x, &v->y, margin); + max_idx = i; + } + for (int i = max_idx; i >= 0; i--) { + v += car_space_to_full_frame(s, line_x[i], -line_y[i] + y_off, -line_z[i] + z_off, &v->x, &v->y, margin); + } + pvd->cnt = v - pvd->v; + assert(pvd->cnt < std::size(pvd->v)); +} + +static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { + UIScene &scene = s->scene; + const float max_distance = fmin(model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + // update lane lines + const auto lane_lines = model.getLaneLines(); + const auto lane_line_probs = model.getLaneLineProbs(); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + scene.lane_line_probs[i] = lane_line_probs[i]; + update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 1.22, &scene.lane_line_vertices[i], max_distance); + } + + // update road edges + const auto road_edges = model.getRoadEdges(); + const auto road_edge_stds = model.getRoadEdgeStds(); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + scene.road_edge_stds[i] = road_edge_stds[i]; + update_line_data(s, road_edges[i], 0.025, 1.22, &scene.road_edge_vertices[i], max_distance); + } + + // update path + const float lead_d = scene.lead_data[0].getStatus() ? scene.lead_data[0].getDRel() * 2. : MAX_DRAW_DISTANCE; + float path_length = (lead_d > 0.) ? lead_d - fmin(lead_d * 0.35, 10.) : MAX_DRAW_DISTANCE; + path_length = fmin(path_length, max_distance); + update_line_data(s, model.getPosition(), 0.5, 0, &scene.track_vertices, path_length); +} + void update_sockets(UIState *s) { UIScene &scene = s->scene; @@ -170,23 +215,7 @@ void update_sockets(UIState *s) { } } if (sm.updated("modelV2")) { - scene.model = sm["modelV2"].getModelV2(); - scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); - for (int ll_idx = 0; ll_idx < 4; ll_idx++) { - if (scene.model.getLaneLineProbs().size() > ll_idx) { - scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx]; - } else { - scene.lane_line_probs[ll_idx] = 0.0; - } - } - - for (int re_idx = 0; re_idx < 2; re_idx++) { - if (scene.model.getRoadEdgeStds().size() > re_idx) { - scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx]; - } else { - scene.road_edge_stds[re_idx] = 1.0; - } - } + update_model(s, sm["modelV2"].getModelV2()); } if (sm.updated("uiLayoutState")) { auto data = sm["uiLayoutState"].getUiLayoutState(); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 78fd1ca1c..767352ef7 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -86,11 +86,18 @@ static std::map bg_colors = { }; typedef struct { - float x[TRAJECTORY_SIZE]; - float y[TRAJECTORY_SIZE]; - float z[TRAJECTORY_SIZE]; -} line; + float x, y; +} vertex_data; +typedef struct { + vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; + int cnt; +} line_vertices_data; + +typedef struct { + vertex_data v[TRACK_POINTS_MAX_CNT]; + int cnt; +} track_vertices_data; typedef struct UIScene { @@ -117,34 +124,15 @@ typedef struct UIScene { cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; cereal::DMonitoringState::Reader dmonitoring_state; - cereal::ModelDataV2::Reader model; - line path; - line outer_left_lane_line; - line left_lane_line; - line right_lane_line; - line outer_right_lane_line; - line left_road_edge; - line right_road_edge; - float max_distance; + + // modelV2 float lane_line_probs[4]; float road_edge_stds[2]; + track_vertices_data track_vertices; + line_vertices_data lane_line_vertices[4]; + line_vertices_data road_edge_vertices[2]; } UIScene; -typedef struct { - float x, y; -} vertex_data; - -typedef struct { - vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; - int cnt; -} line_vertices_data; - -typedef struct { - vertex_data v[TRACK_POINTS_MAX_CNT]; - int cnt; -} track_vertices_data; - - typedef struct UIState { // framebuffer FramebufferState *fb; @@ -201,10 +189,6 @@ typedef struct UIState { bool alert_blinked; float alert_blinking_alpha; - track_vertices_data track_vertices; - line_vertices_data lane_line_vertices[4]; - line_vertices_data road_edge_vertices[2]; - Rect video_rect; } UIState;