Traffic b2 (#1345)

* add traffic convention

* hope this work

* no comment

* latest and gratest

* big gru model

* 1af55c7d-ee15-414a-9e98-a0cb08c3441f/75

* much later in training

* wrong temporal size

* converged

* fix lane changes

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};
+        float vec_desire[DESIRE_LEN] = {0};
-        if (desire >= 0 && desire < DESIRE_SIZE) {
+        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);

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));
+  s->prev_desire = (float*)malloc(DESIRE_LEN * sizeof(float));
-  for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
+  for (int i = 0; i < DESIRE_LEN; i++) s->prev_desire[i] = 0.0;
-  s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
+  s->pulse_desire = (float*)malloc(DESIRE_LEN * sizeof(float));
-  for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0;
+  for (int i = 0; i < DESIRE_LEN; i++) s->pulse_desire[i] = 0.0;
-  s->m->addDesire(s->pulse_desire, DESIRE_SIZE);
+  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

selfdrive/modeld/models/driving.h

@@ -4,10 +4,7 @@
 // gate this here
 #define TEMPORAL
 #define DESIRE
-
+#define TRAFFIC_CONVENTION
-#ifdef DESIRE
-  #define DESIRE_SIZE 8
-#endif
 
 #ifdef QCOM
 #include <eigen3/Eigen/Dense>
@@ -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);
 

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) {}
 };
 

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) {

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<zdl::DlSystem::IUserBuffer> addExtra(float *state, int state_size, int idx);
   std::unique_ptr<zdl::DlSystem::IUserBuffer> recurrentBuffer;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> trafficConventionBuffer;
   std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
 };
 

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);

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);

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];
-
+    const float8 ysf = convert_float8(ys);
-    // y = (x - 128) / 128
-    const float8 ysf = (convert_float8(ys) - 128.f) * 0.0078125f;
 
     // 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];
-
+  const float8 outv  = convert_float8(inv);
-  // y = (x - 128) / 128
-  const float8 outv  = (convert_float8(inv) - 128.f) * 0.0078125f;
   out[gid + out_offset / 8] = outv;
 }