Getting PC openpilot working again (#1650)

* fixup keras_runner to work and be nicer

* truncate the lane lines based on the valid len
old-commit-hash: 7770041f92

selfdrive/common/modeldata.h

@@ -13,6 +13,7 @@ typedef struct PathData {
   float std;
   float stds[MODEL_PATH_DISTANCE];
   float poly[POLYFIT_DEGREE];
+  float validLen;
 } PathData;
 
 typedef struct LeadData {

selfdrive/modeld/modeld.cc

@@ -107,7 +107,7 @@ int main(int argc, char **argv) {
 
     char cBuffer[1024];
     clGetPlatformInfo(platform_id[clPlatform], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
-    LOGD("got %d opencl platform(s), using %s", num_platforms, cBuffer);
+    LOG("got %d opencl platform(s), using %s", num_platforms, cBuffer);
 
     err = clGetDeviceIDs(platform_id[clPlatform], CL_DEVICE_TYPE_DEFAULT, 1,
                          &device_id, &num_devices);
@@ -118,6 +118,7 @@ int main(int argc, char **argv) {
 
     q = clCreateCommandQueue(context, device_id, 0, &err);
     assert(err == 0);
+    LOGD("opencl init complete");
   }
 
   // init the models
@@ -192,7 +193,7 @@ int main(int argc, char **argv) {
         model_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
         posenet_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
 
-        LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
+        LOG("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
         last = mt1;
       }
 

selfdrive/modeld/runners/keras_runner.py

@@ -5,12 +5,9 @@ from __future__ import print_function
 import tensorflow as tf  # pylint: disable=import-error
 import os
 import sys
-import tensorflow.keras as keras  # pylint: disable=import-error
 import numpy as np
-from tensorflow.keras.models import Model  # pylint: disable=import-error
 from tensorflow.keras.models import load_model  # pylint: disable=import-error
 
-
 def read(sz):
   dd = []
   gt = 0
@@ -19,21 +16,24 @@ def read(sz):
     assert(len(st) > 0)
     dd.append(st)
     gt += len(st)
-  return np.fromstring(b''.join(dd), dtype=np.float32)
+  return np.frombuffer(b''.join(dd), dtype=np.float32)
-
 
 def write(d):
   os.write(1, d.tobytes())
 
-
 def run_loop(m):
-  isize = m.inputs[0].shape[1]
+  ishapes = [[1]+ii.shape[1:] for ii in m.inputs]
-  osize = m.outputs[0].shape[1]
+  print("ready to run keras model", ishapes, file=sys.stderr)
-  print("ready to run keras model %d -> %d" % (isize, osize), file=sys.stderr)
   while 1:
-    idata = read(isize).reshape((1, isize))
+    inputs = []
-    ret = m.predict_on_batch(idata)
+    for shp in ishapes:
-    write(ret)
+      ts = np.product(shp)
+      #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
+      inputs.append(read(ts).reshape(shp))
+    ret = m.predict_on_batch(inputs)
+    #print(ret, file=sys.stderr)
+    for r in ret:
+      write(r)
 
 
 if __name__ == "__main__":
@@ -45,17 +45,6 @@ if __name__ == "__main__":
 
   m = load_model(sys.argv[1])
   print(m, file=sys.stderr)
-  bs = [int(np.product(ii.shape[1:])) for ii in m.inputs]
+
-  ri = keras.layers.Input((sum(bs),))
-
-  tii = []
-  acc = 0
-  for i, ii in enumerate(m.inputs):
-    print(ii, file=sys.stderr)
-    ti = keras.layers.Lambda(lambda x, i=i: x[:, acc:acc + bs[i]], output_shape=(1, bs[i]))(ri)
-    acc += bs[i]
-    tr = keras.layers.Reshape(ii.shape[1:])(ti)
-    tii.append(tr)
-  no = keras.layers.Concatenate()(m(tii))
-  m = Model(inputs=ri, outputs=[no])
   run_loop(m)
+

selfdrive/modeld/runners/tfmodel.cc

@@ -62,20 +62,20 @@ void TFModel::pwrite(float *buf, int size) {
     cbuf += err;
     tw -= err;
   }
-  //printf("host write done\n");
+  LOGD("host write of size %d done", size);
 }
 
 void TFModel::pread(float *buf, int size) {
   char *cbuf = (char *)buf;
   int tr = size*sizeof(float);
   while (tr > 0) {
+    LOGD("host read remaining %d/%d", tr, size*sizeof(float));
     int err = read(pipeout[0], cbuf, tr);
-    //printf("host read %d/%d\n", err, tr);
     assert(err >= 0);
     cbuf += err;
     tr -= err;
   }
-  //printf("host read done\n");
+  LOGD("host read done");
 }
 
 void TFModel::addRecurrent(float *state, int state_size) {
@@ -99,12 +99,12 @@ void TFModel::execute(float *net_input_buf, int buf_size) {
   if (desire_input_buf != NULL) {
     pwrite(desire_input_buf, desire_state_size);
   }
-  if (rnn_input_buf != NULL) {
-    pwrite(rnn_input_buf, rnn_state_size);
-  }
   if (traffic_convention_input_buf != NULL) {
     pwrite(traffic_convention_input_buf, traffic_convention_size);
   }
+  if (rnn_input_buf != NULL) {
+    pwrite(rnn_input_buf, rnn_state_size);
+  }
   pread(output, output_size);
 }
 

selfdrive/ui/paint.cc

@@ -247,16 +247,6 @@ static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) {
   nvgFill(s->vg);
 }
 
-static void draw_steering(UIState *s, float curvature) {
-  float points[50];
-  for (int i = 0; i < 50; i++) {
-    float y_actual = i * tan(asin(clamp(i * curvature, -0.999, 0.999)) / 2.);
-    points[i] = y_actual;
-  }
-
-  // ui_draw_lane_edge(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5);
-}
-
 static void draw_frame(UIState *s) {
   const UIScene *scene = &s->scene;
 
@@ -298,9 +288,10 @@ static inline bool valid_frame_pt(UIState *s, float x, float y) {
   return x >= 0 && x <= s->rgb_width && y >= 0 && y <= s->rgb_height;
 
 }
-static void update_lane_line_data(UIState *s, const float *points, float off, bool is_ghost, model_path_vertices_data *pvd) {
+static void update_lane_line_data(UIState *s, const float *points, float off, bool is_ghost, model_path_vertices_data *pvd, float valid_len) {
   pvd->cnt = 0;
-  for (int i = 0; i < MODEL_PATH_MAX_VERTICES_CNT / 2; i++) {
+  int rcount = fmin(MODEL_PATH_MAX_VERTICES_CNT / 2, valid_len);
+  for (int i = 0; i < rcount; i++) {
     float px = (float)i;
     float py = points[i] - off;
     const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
@@ -311,7 +302,7 @@ static void update_lane_line_data(UIState *s, const float *points, float off, bo
     pvd->v[pvd->cnt].y = p_full_frame.v[1];
     pvd->cnt += 1;
   }
-  for (int i = MODEL_PATH_MAX_VERTICES_CNT / 2; i > 0; i--) {
+  for (int i = rcount; i > 0; i--) {
     float px = (float)i;
     float py = is_ghost?(points[i]-off):(points[i]+off);
     const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
@@ -325,10 +316,10 @@ static void update_lane_line_data(UIState *s, const float *points, float off, bo
 }
 
 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, false, pstart);
+  update_lane_line_data(s, path.points, 0.025*path.prob, false, pstart, path.validLen);
   float var = fmin(path.std, 0.7);
-  update_lane_line_data(s, path.points, -var, true, pstart + 1);
+  update_lane_line_data(s, path.points, -var, true, pstart + 1, path.validLen);
-  update_lane_line_data(s, path.points, var, true, pstart + 2);
+  update_lane_line_data(s, path.points, var, true, pstart + 2, path.validLen);
 }
 
 static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_data *pstart, NVGcolor color) {

selfdrive/ui/ui.cc

@@ -279,6 +279,8 @@ static void read_path(PathData& p, const cereal::ModelData::PathData::Reader &pa
   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) {