diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index d1a3bd2615..4dd7186f40 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 d84bfb0e33..f3b6d1a016 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 5b3ded0ef8..ee75f8ed01 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 78fd1ca1c5..767352ef72 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;