sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

populate model data in update_socket

Browse Source
pull/19657/head
deanlee 4 years ago
parent e25db7e28c
commit 6fa500dbce
3 changed files with 73 additions and 87 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 61
      selfdrive/ui/paint.cc
  2. 49
      selfdrive/ui/ui.cc
  3. 50
      selfdrive/ui/ui.hpp

61
selfdrive/ui/paint.cc
Unescape View File

@ -177,69 +177,24 @@ static void draw_frame(UIState *s) {
glBindVertexArray(0);
}
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) {
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 UIScene *scene = &s->scene;
// update lane lines
const auto lane_lines = scene->model.getLaneLines();
const auto lane_line_probs = scene->model.getLaneLineProbs();
for (int i = 0; i < std::size(s->lane_line_vertices); i++) {
update_line_data(s, lane_lines[i], 0.025 * lane_line_probs[i], 1.22, &s->lane_line_vertices[i], scene->max_distance);
}
// update road edges
const auto road_edges = scene->model.getRoadEdges();
for (int i = 0; i < std::size(s->road_edge_vertices); i++) {
update_line_data(s, road_edges[i], 0.025, 1.22, &s->road_edge_vertices[i], scene->max_distance);
}
// update path
const float lead_d = s->sm->updated("radarState") ? 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, scene->max_distance);
update_line_data(s, scene->model.getPosition(), 0.5, 0, &s->track_vertices, path_length);
}
static void ui_draw_vision_lane_lines(UIState *s) {
const UIScene *scene = &s->scene;
if (s->sm->updated("modelV2")) {
update_model(s);
}
const UIScene &scene = s->scene;
// paint lanelines
const auto lane_line_probs = scene->model.getLaneLineProbs();
for (int i = 0; i < std::size(s->lane_line_vertices); i++) {
NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, lane_line_probs[i]);
ui_draw_line(s, s->lane_line_vertices[i].v, s->lane_line_vertices[i].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
const auto road_edge_stds = scene->model.getRoadEdgeStds();
for (int i = 0; i < std::size(s->road_edge_vertices); i++) {
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0));
ui_draw_line(s, s->road_edge_vertices[i].v, s->road_edge_vertices[i].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<float>(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
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, s->track_vertices.v, s->track_vertices.cnt, nullptr, &track_bg);
ui_draw_line(s, scene.track_vertices.v, scene.track_vertices.cnt, nullptr, &track_bg);
}
// Draw all world space objects.

49
selfdrive/ui/ui.cc
Unescape View File

@ -16,6 +16,8 @@
#include "ui.hpp"
#include "paint.hpp"
extern 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);
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 +106,50 @@ destroy:
s->vision_connected = false;
}
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) {
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 = s->sm->updated("radarState") ? 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,8 +216,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);
update_model(s, sm["modelV2"].getModelV2());
}
if (sm.updated("uiLayoutState")) {
auto data = sm["uiLayoutState"].getUiLayoutState();

50
selfdrive/ui/ui.hpp
Unescape View File

@ -86,11 +86,18 @@ static std::map<UIStatus, NVGcolor> 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,31 +124,14 @@ 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;
} 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;
// 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 UIState {
// framebuffer
@ -199,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;

Write Preview
Loading…
Cancel
Save