diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 555a75614c..111e20a413 100644 --- a/selfdrive/common/modeldata.h +++ b/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 { diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 793474e652..5404f61b3c 100644 --- a/selfdrive/modeld/modeld.cc +++ b/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; } diff --git a/selfdrive/modeld/runners/keras_runner.py b/selfdrive/modeld/runners/keras_runner.py index c692ac5988..0a9eddeac7 100755 --- a/selfdrive/modeld/runners/keras_runner.py +++ b/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] - osize = m.outputs[0].shape[1] - print("ready to run keras model %d -> %d" % (isize, osize), file=sys.stderr) + ishapes = [[1]+ii.shape[1:] for ii in m.inputs] + print("ready to run keras model", ishapes, file=sys.stderr) while 1: - idata = read(isize).reshape((1, isize)) - ret = m.predict_on_batch(idata) - write(ret) + inputs = [] + for shp in ishapes: + 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) + diff --git a/selfdrive/modeld/runners/tfmodel.cc b/selfdrive/modeld/runners/tfmodel.cc index c955e54a77..c96fedfbf0 100644 --- a/selfdrive/modeld/runners/tfmodel.cc +++ b/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); } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 3be25899b0..60e403aff6 100644 --- a/selfdrive/ui/paint.cc +++ b/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 + 2); + update_lane_line_data(s, path.points, -var, true, pstart + 1, path.validLen); + 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) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1807adee70..96ec3571af 100644 --- a/selfdrive/ui/ui.cc +++ b/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) {