From ebd4f68cbafc8d03a9965a06b89998e0f29b98d7 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 18 Aug 2020 21:26:43 +0800 Subject: [PATCH] ui: refactor model related functions (#2026) * remove read_model * remove structs for c-capnp * remove duplicate #define from modeld * add function fill_path_points * fix Indentation * use MODEL_PATH_DISTANCE instead of 192 * fix type use validLen * rename left_path_points&right_path_points to xxx_lane_points old-commit-hash: f8ab6bd009410a9f7ab9c30ce7b72a82c6d2507b --- selfdrive/common/modeldata.h | 36 +------------------------ selfdrive/modeld/models/driving.cc | 4 +-- selfdrive/modeld/models/driving.h | 6 +---- selfdrive/ui/paint.cc | 31 ++++++++++------------ selfdrive/ui/ui.cc | 42 ++++++++---------------------- selfdrive/ui/ui.hpp | 6 +++-- 6 files changed, 33 insertions(+), 92 deletions(-) diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 111e20a413..77f4a1b7a1 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,41 +1,7 @@ -#ifndef MODELDATA_H -#define MODELDATA_H +#pragma once #define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 #define DESIRE_PRED_SIZE 32 #define OTHER_META_SIZE 4 - -typedef struct PathData { - float points[MODEL_PATH_DISTANCE]; - float prob; - float std; - float stds[MODEL_PATH_DISTANCE]; - float poly[POLYFIT_DEGREE]; - float validLen; -} PathData; - -typedef struct LeadData { - float dist; - float prob; - float std; - float rel_y; - float rel_y_std; - float rel_v; - float rel_v_std; - float rel_a; - float rel_a_std; -} LeadData; - -typedef struct ModelData { - PathData path; - PathData left_lane; - PathData right_lane; - LeadData lead; - LeadData lead_future; - float meta[OTHER_META_SIZE + DESIRE_PRED_SIZE]; - float speed[SPEED_PERCENTILES]; -} ModelData; - -#endif diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index f58e7b1964..71e4fcae3a 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -162,8 +162,8 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo float prob; float valid_len; - // clamp to 5 and 192 - valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2])); + // clamp to 5 and MODEL_PATH_DISTANCE + valid_len = fmin(MODEL_PATH_DISTANCE, fmax(5, data[MODEL_PATH_DISTANCE*2])); for (int i=0; iscene; - const PathData path = scene->model.path; + const float *points = scene->path_points; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; @@ -150,6 +150,7 @@ static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) float lead_d = scene->lead_data[0].getDRel()*2.; float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20. :(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.; + path_height = fmin(path_height, scene->model.getPath().getValidLen()); pvd->cnt = 0; // left side up for (int i=0; i<=path_height; i++) { @@ -160,7 +161,7 @@ static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) py = mpc_y_coords[i] - off; } else { px = lerp(i+1.0, i, i/100.0); - py = path.points[i] - off; + py = points[i] - off; } vec4 p_car_space = (vec4){{px, py, 0., 1.}}; @@ -182,7 +183,7 @@ static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) py = mpc_y_coords[i] + off; } else { px = lerp(i+1.0, i, i/100.0); - py = path.points[i] + off; + py = points[i] + off; } vec4 p_car_space = (vec4){{px, py, 0., 1.}}; @@ -207,7 +208,6 @@ static void update_all_track_data(UIState *s) { } } - static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { if (pvd->cnt == 0) return; @@ -302,13 +302,12 @@ static void update_lane_line_data(UIState *s, const float *points, float off, mo } } -static void update_all_lane_lines_data(UIState *s, const PathData &path, model_path_vertices_data *pstart) { - update_lane_line_data(s, path.points, 0.025*path.prob, pstart, path.validLen); - float var = fmin(path.std, 0.7); - update_lane_line_data(s, path.points, var, pstart + 1, path.validLen); +static void update_all_lane_lines_data(UIState *s, const cereal::ModelData::PathData::Reader &path, const float *points, model_path_vertices_data *pstart) { + update_lane_line_data(s, points, 0.025*path.getProb(), pstart, path.getValidLen()); + update_lane_line_data(s, points, fmin(path.getStd(), 0.7), pstart + 1, path.getValidLen()); } -static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_data *pstart, NVGcolor color) { +static void ui_draw_lane(UIState *s, model_path_vertices_data *pstart, NVGcolor color) { ui_draw_lane_line(s, pstart, color); color.a /= 25; ui_draw_lane_line(s, pstart + 1, color); @@ -318,20 +317,18 @@ static void ui_draw_vision_lanes(UIState *s) { const UIScene *scene = &s->scene; model_path_vertices_data *pvd = &s->model_path_vertices[0]; if(s->sm->updated("model")) { - update_all_lane_lines_data(s, scene->model.left_lane, pvd); - update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT); + update_all_lane_lines_data(s, scene->model.getLeftLane(), scene->left_lane_points, pvd); + update_all_lane_lines_data(s, scene->model.getRightLane(), scene->right_lane_points, pvd + MODEL_LANE_PATH_CNT); } // Draw left lane edge ui_draw_lane( - s, &scene->model.left_lane, - pvd, - nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); + s, pvd, + nvgRGBAf(1.0, 1.0, 1.0, scene->model.getLeftLane().getProb())); // Draw right lane edge ui_draw_lane( - s, &scene->model.right_lane, - pvd + MODEL_LANE_PATH_CNT, - nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); + s, pvd + MODEL_LANE_PATH_CNT, + nvgRGBAf(1.0, 1.0, 1.0, scene->model.getRightLane().getProb())); if(s->sm->updated("radarState")) { update_all_track_data(s); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b8edfc4e2e..5eb00a2edb 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -227,42 +227,19 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->limit_set_speed_timeout = UI_FREQ; } -static void read_path(PathData& p, const cereal::ModelData::PathData::Reader &pathp) { - p = {}; - - p.prob = pathp.getProb(); - p.std = pathp.getStd(); - - auto polyp = pathp.getPoly(); - for (int i = 0; i < POLYFIT_DEGREE; i++) { - p.poly[i] = polyp[i]; - } - - // Compute points locations - for (int i = 0; i < MODEL_PATH_DISTANCE; i++) { - p.points[i] = p.poly[0] * (i*i*i) + p.poly[1] * (i*i)+ p.poly[2] * i + p.poly[3]; - } - - p.validLen = pathp.getValidLen(); -} - -static void read_model(ModelData &d, const cereal::ModelData::Reader &model) { - d = {}; - read_path(d.path, model.getPath()); - read_path(d.left_lane, model.getLeftLane()); - read_path(d.right_lane, model.getRightLane()); - auto leadd = model.getLead(); - d.lead = (LeadData){ - .dist = leadd.getDist(), .prob = leadd.getProb(), .std = leadd.getStd(), - }; -} - static void update_status(UIState *s, int status) { if (s->status != status) { s->status = status; } } +static inline void fill_path_points(const cereal::ModelData::PathData::Reader &path, float *points) { + const capnp::List::Reader &poly = path.getPoly(); + for (int i = 0; i < path.getValidLen(); i++) { + points[i] = poly[0] * (i * i * i) + poly[1] * (i * i) + poly[2] * i + poly[3]; + } +} + void handle_message(UIState *s, SubMaster &sm) { UIScene &scene = s->scene; if (s->started && sm.updated("controlsState")) { @@ -323,7 +300,10 @@ void handle_message(UIState *s, SubMaster &sm) { } } if (sm.updated("model")) { - read_model(scene.model, sm["model"].getModel()); + scene.model = sm["model"].getModel(); + fill_path_points(scene.model.getPath(), scene.path_points); + fill_path_points(scene.model.getLeftLane(), scene.left_lane_points); + fill_path_points(scene.model.getRightLane(), scene.right_lane_points); } // else if (which == cereal::Event::LIVE_MPC) { // auto data = event.getLiveMpc(); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 24a982db17..2d6d9ddc06 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -92,8 +92,6 @@ typedef struct UIScene { int frontview; int fullview; - ModelData model; - float mpc_x[50]; float mpc_y[50]; @@ -126,6 +124,10 @@ typedef struct UIScene { cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; cereal::DMonitoringState::Reader dmonitoring_state; + cereal::ModelData::Reader model; + float left_lane_points[MODEL_PATH_DISTANCE]; + float path_points[MODEL_PATH_DISTANCE]; + float right_lane_points[MODEL_PATH_DISTANCE]; } UIScene; typedef struct {