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

@@ -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

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; i<MODEL_PATH_DISTANCE; i++) {
     points_arr[i] = data[i] + offset;
     stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;

selfdrive/modeld/models/driving.h

@@ -9,6 +9,7 @@
 
 #include "common/mat.h"
 #include "common/util.h"
+#include "common/modeldata.h"
 
 #include "commonmodel.h"
 #include "runners/run.h"
@@ -22,13 +23,8 @@
 #define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
 #define MODEL_NAME "supercombo_dlc"
 
-#define MODEL_PATH_DISTANCE 192
-#define POLYFIT_DEGREE 4
-#define SPEED_PERCENTILES 10
 #define DESIRE_LEN 8
 #define TRAFFIC_CONVENTION_LEN 2
-#define DESIRE_PRED_SIZE 32
-#define OTHER_META_SIZE 4
 #define LEAD_MDN_N 5 // probs for 5 groups
 #define MDN_VALS 4 // output xyva for each lead group
 #define SELECTION 3 //output 3 group (lead now, in 2s and 6s)

selfdrive/ui/paint.cc

@@ -142,7 +142,7 @@ static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, N
 
 static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) {
   const UIScene *scene = &s->scene;
-  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);

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<float>::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();

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 {