diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 2159cd73..e02d1ceb 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:aa0ec9d98f3dbc36a1932eb4964cd6f0c98967ef60cdbd2ed609d548442ed101 -size 26664753 +oid sha256:37868e7bf823cc22d18dc3277def1667337cb474b4a96f38ee425561ec17ca04 +size 78605178 diff --git a/models/supercombo.keras b/models/supercombo.keras index 8f6c9ce2..43140d71 100644 --- a/models/supercombo.keras +++ b/models/supercombo.keras @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b5a5a5e54297cbfd0f0731447ad6907506dbc17eb48b3bdd0cb48f81a72143c8 -size 27524584 +oid sha256:b5b0da75fdba32db6da33651dd3eb572755ff5c7f77b7ff03a895334cfcec47c +size 79200720 diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index fa49d1b5..7a2a85d5 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -205,8 +205,8 @@ int main(int argc, char **argv) { double mt1 = 0, mt2 = 0; if (run_model_this_iter) { - float vec_desire[DESIRE_SIZE] = {0}; - if (desire >= 0 && desire < DESIRE_SIZE) { + float vec_desire[DESIRE_LEN] = {0}; + if (desire >= 0 && desire < DESIRE_LEN) { vec_desire[desire] = 1.0; } @@ -219,7 +219,7 @@ int main(int argc, char **argv) { ModelDataRaw model_buf = model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height, - model_transform, NULL, vec_desire); + model_transform, NULL, vec_desire, NULL); mt2 = millis_since_boot(); model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 2033ee8e..df84e4fc 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -18,7 +18,7 @@ #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL - #define TEMPORAL_SIZE 512 + #define TEMPORAL_SIZE 1024 #else #define TEMPORAL_SIZE 0 #endif @@ -42,11 +42,17 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #endif #ifdef DESIRE - s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); - for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; - s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); - for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0; - s->m->addDesire(s->pulse_desire, DESIRE_SIZE); + s->prev_desire = (float*)malloc(DESIRE_LEN * sizeof(float)); + for (int i = 0; i < DESIRE_LEN; i++) s->prev_desire[i] = 0.0; + s->pulse_desire = (float*)malloc(DESIRE_LEN * sizeof(float)); + for (int i = 0; i < DESIRE_LEN; i++) s->pulse_desire[i] = 0.0; + s->m->addDesire(s->pulse_desire, DESIRE_LEN); +#endif + +#ifdef TRAFFIC_CONVENTION + s->traffic_convention = (float*)malloc(TRAFFIC_CONVENTION_LEN * sizeof(float)); + for (int i = 0; i < TRAFFIC_CONVENTION_LEN; i++) s->traffic_convention[i] = 0.0; + s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif // Build Vandermonde matrix @@ -61,18 +67,27 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock, float *desire_in) { + mat3 transform, void* sock, + float *desire_in, float *traffic_convention_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_SIZE; i++) { + for (int i = 0; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge - if (desire_in[i] - s->desire[i] == 1) { + if (desire_in[i] - s->prev_desire[i] > .99) { s->pulse_desire[i] = desire_in[i]; } else { s->pulse_desire[i] = 0.0; } - s->desire[i] = desire_in[i]; + s->prev_desire[i] = desire_in[i]; + } + } +#endif + +#ifdef TRAFFIC_CONVENTION + if (traffic_convention_in != NULL) { + for (int i = 0; i < TRAFFIC_CONVENTION_LEN; i++) { + s->traffic_convention[i] = traffic_convention_in[i]; } } #endif diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 742eb6ce..45d5396e 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -4,10 +4,7 @@ // gate this here #define TEMPORAL #define DESIRE - -#ifdef DESIRE - #define DESIRE_SIZE 8 -#endif +#define TRAFFIC_CONVENTION #ifdef QCOM #include @@ -35,6 +32,7 @@ #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 @@ -64,16 +62,19 @@ typedef struct ModelState { float *input_frames; RunModel *m; #ifdef DESIRE - float *desire; + float *prev_desire; float *pulse_desire; #endif +#ifdef TRAFFIC_CONVENTION + float *traffic_convention; +#endif } ModelState; void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal); ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height, - mat3 transform, void* sock, float *desire_in); + mat3 transform, void* sock, float *desire_in, float *traffic_convention_in); void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 8ddc2503..d3889aaa 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -5,6 +5,7 @@ class RunModel { public: virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} + virtual void addTrafficConvention(float *state, int state_size) {} virtual void execute(float *net_input_buf, int buf_size) {} }; diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index c954f9b9..084a7c77 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -94,7 +94,11 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int ru } void SNPEModel::addRecurrent(float *state, int state_size) { - recurrentBuffer = this->addExtra(state, state_size, 2); + recurrentBuffer = this->addExtra(state, state_size, 3); +} + +void SNPEModel::addTrafficConvention(float *state, int state_size) { + trafficConventionBuffer = this->addExtra(state, state_size, 2); } void SNPEModel::addDesire(float *state, int state_size) { diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index f99acd09..9289444b 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -24,6 +24,7 @@ public: if (model_data) free(model_data); } void addRecurrent(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); void execute(float *net_input_buf, int buf_size); private: @@ -44,6 +45,7 @@ private: // recurrent and desire std::unique_ptr addExtra(float *state, int state_size, int idx); std::unique_ptr recurrentBuffer; + std::unique_ptr trafficConventionBuffer; std::unique_ptr desireBuffer; }; diff --git a/selfdrive/modeld/runners/tfmodel.cc b/selfdrive/modeld/runners/tfmodel.cc index 0f50614b..1260503f 100644 --- a/selfdrive/modeld/runners/tfmodel.cc +++ b/selfdrive/modeld/runners/tfmodel.cc @@ -88,6 +88,11 @@ void TFModel::addDesire(float *state, int state_size) { desire_state_size = state_size; } +void TFModel::addTrafficConvention(float *state, int state_size) { + traffic_convention_input_buf = state; + traffic_convention_size = state_size; +} + void TFModel::execute(float *net_input_buf, int buf_size) { // order must be this pwrite(net_input_buf, buf_size); diff --git a/selfdrive/modeld/runners/tfmodel.h b/selfdrive/modeld/runners/tfmodel.h index 66e50b4a..8e626385 100644 --- a/selfdrive/modeld/runners/tfmodel.h +++ b/selfdrive/modeld/runners/tfmodel.h @@ -12,6 +12,7 @@ public: ~TFModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); void execute(float *net_input_buf, int buf_size); private: int proc_pid; @@ -23,6 +24,8 @@ private: int rnn_state_size; float *desire_input_buf = NULL; int desire_state_size; + float *traffic_convention_input_buf = NULL; + int traffic_convention_size; // pipe to communicate to keras subprocess void pread(float *buf, int size); diff --git a/selfdrive/modeld/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl index e0154291..fc7655b5 100644 --- a/selfdrive/modeld/transforms/loadyuv.cl +++ b/selfdrive/modeld/transforms/loadyuv.cl @@ -9,9 +9,7 @@ __kernel void loadys(__global uchar8 const * const Y, const int ox = ois % TRANSFORMED_WIDTH; const uchar8 ys = Y[gid]; - - // y = (x - 128) / 128 - const float8 ysf = (convert_float8(ys) - 128.f) * 0.0078125f; + const float8 ysf = convert_float8(ys); // 02 // 13 @@ -36,8 +34,6 @@ __kernel void loaduv(__global uchar8 const * const in, { const int gid = get_global_id(0); const uchar8 inv = in[gid]; - - // y = (x - 128) / 128 - const float8 outv = (convert_float8(inv) - 128.f) * 0.0078125f; + const float8 outv = convert_float8(inv); out[gid + out_offset / 8] = outv; }