Navmodel and driving style: update runner (#26762)

* Navmodel and driving style: update runner

* No driving style yet

* thneedrunner without extra inputs

* nav feature ref

selfdrive/modeld/modeld.cc

@@ -15,6 +15,7 @@
 #include "common/util.h"
 #include "system/hardware/hw.h"
 #include "selfdrive/modeld/models/driving.h"
+#include "selfdrive/modeld/models/nav.h"
 
 
 ExitHandler do_exit;
@@ -72,6 +73,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
   mat3 model_transform_main = {};
   mat3 model_transform_extra = {};
   bool live_calib_seen = false;
+  float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0};
+  float nav_features[NAV_FEATURE_LEN] = {0};
 
   VisionBuf *buf_main = nullptr;
   VisionBuf *buf_extra = nullptr;
@@ -151,7 +154,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
     }
 
     double mt1 = millis_since_boot();
-    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only);
+    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only);
     double mt2 = millis_since_boot();
     float model_execution_time = (mt2 - mt1) / 1000.0;
 

selfdrive/modeld/models/driving.cc

@@ -46,10 +46,19 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
 #ifdef TRAFFIC_CONVENTION
   s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
 #endif
+
+#ifdef DRIVING_STYLE
+  s->m->addDrivingStyle(s->driving_style, DRIVING_STYLE_LEN);
+#endif
+
+#ifdef NAV
+  s->m->addNavFeatures(s->nav_features, NAV_FEATURE_LEN);
+#endif
+
 }
 
 ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) {
+                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) {
 #ifdef DESIRE
   std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
   if (desire_in != NULL) {
@@ -67,6 +76,14 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
 LOGT("Desire enqueued");
 #endif
 
+#ifdef NAV
+  std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN);
+#endif
+
+#ifdef DRIVING_STYLE
+  std::memcpy(s->driving_style, driving_style, sizeof(float)*DRIVING_STYLE_LEN);
+#endif
+
   int rhd_idx = is_rhd;
   s->traffic_convention[rhd_idx] = 1.0;
   s->traffic_convention[1-rhd_idx] = 0.0;

selfdrive/modeld/models/driving.h

@@ -14,6 +14,7 @@
 #include "common/modeldata.h"
 #include "common/util.h"
 #include "selfdrive/modeld/models/commonmodel.h"
+#include "selfdrive/modeld/models/nav.h"
 #include "selfdrive/modeld/runners/run.h"
 
 constexpr int FEATURE_LEN = 128;
@@ -21,6 +22,7 @@ constexpr int HISTORY_BUFFER_LEN = 99;
 constexpr int DESIRE_LEN = 8;
 constexpr int DESIRE_PRED_LEN = 4;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
+constexpr int DRIVING_STYLE_LEN = 12;
 constexpr int MODEL_FREQ = 20;
 
 constexpr int DISENGAGE_LEN = 5;
@@ -259,11 +261,17 @@ struct ModelState {
 #ifdef TRAFFIC_CONVENTION
   float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
 #endif
+#ifdef DRIVING_STYLE
+  float driving_style[DRIVING_STYLE_LEN] = {};
+#endif
+#ifdef NAV
+  float nav_features[NAV_FEATURE_LEN] = {};
+#endif
 };
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only);
+                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only);
 void model_free(ModelState* s);
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,

selfdrive/modeld/models/nav.cc

@@ -9,11 +9,11 @@
 
 
 void navmodel_init(NavModelState* s) {
-#ifdef USE_ONNX_MODEL
-  s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
-#else
-  s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
-#endif
+  #ifdef USE_ONNX_MODEL
+    s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
+  #else
+    s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
+  #endif
 }
 
 NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) {
@@ -56,7 +56,7 @@ void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &mo
   framed.setDspExecutionTime(model_res.dsp_execution_time);
   framed.setFeatures(to_kj_array_ptr(model_res.features.values));
   framed.setDesirePrediction(to_kj_array_ptr(model_res.desire_pred.values));
-  fill_plan(framed, model_res.plans.get_best_prediction());
+  fill_plan(framed, model_res.plan);
 
   pm.send("navModel", msg);
 }

selfdrive/modeld/models/nav.h

@@ -10,7 +10,6 @@
 constexpr int NAV_INPUT_SIZE = 256*256;
 constexpr int NAV_FEATURE_LEN = 64;
 constexpr int NAV_DESIRE_LEN = 32;
-constexpr int NAV_PLAN_MHP_N = 5;
 
 struct NavModelOutputXY {
   float x;
@@ -21,24 +20,8 @@ static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2);
 struct NavModelOutputPlan {
   std::array<NavModelOutputXY, TRAJECTORY_SIZE> mean;
   std::array<NavModelOutputXY, TRAJECTORY_SIZE> std;
-  float prob;
 };
-static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2 + sizeof(float));
-
-struct NavModelOutputPlans {
-  std::array<NavModelOutputPlan, NAV_PLAN_MHP_N> predictions;
-
-  constexpr const NavModelOutputPlan &get_best_prediction() const {
-    int max_idx = 0;
-    for (int i = 1; i < predictions.size(); i++) {
-      if (predictions[i].prob > predictions[max_idx].prob) {
-        max_idx = i;
-      }
-    }
-    return predictions[max_idx];
-  }
-};
-static_assert(sizeof(NavModelOutputPlans) == sizeof(NavModelOutputPlan)*NAV_PLAN_MHP_N);
+static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2);
 
 struct NavModelOutputDesirePrediction {
   std::array<float, NAV_DESIRE_LEN> values;
@@ -51,12 +34,12 @@ struct NavModelOutputFeatures {
 static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
 
 struct NavModelResult {
-  const NavModelOutputPlans plans;
+  const NavModelOutputPlan plan;
   const NavModelOutputDesirePrediction desire_pred;
   const NavModelOutputFeatures features;
   float dsp_execution_time;
 };
-static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlans) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
+static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
 
 constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float);
 constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1;

selfdrive/modeld/models/navmodel.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:eab4b986e14d7d842d6d5487011c329d356fb56995b2ae7dc7188aefe6df9d97
-size 12285002
+oid sha256:9028b36a591763724e95205b48f37f09260b4434fb1f3c6f228db1710a401aa8
+size 12258591

selfdrive/modeld/models/navmodel_q.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:83d53efc40053b02fe7d3da4ef6213a4a5a1ae4d1bd49c121b9beb6a54ea1148
-size 3154868
+oid sha256:bc7ade56bb4f9525c84a46df22252ea1e321a0518cbcef8bdfc76ccd8ad10b41
+size 3154304

selfdrive/modeld/runners/onnxmodel.cc

@@ -97,6 +97,16 @@ void ONNXModel::addDesire(float *state, int state_size) {
   desire_state_size = state_size;
 }
 
+void ONNXModel::addNavFeatures(float *state, int state_size) {
+  nav_features_input_buf = state;
+  nav_features_size = state_size;
+}
+
+void ONNXModel::addDrivingStyle(float *state, int state_size) {
+    driving_style_input_buf = state;
+    driving_style_size = state_size;
+}
+
 void ONNXModel::addTrafficConvention(float *state, int state_size) {
   traffic_convention_input_buf = state;
   traffic_convention_size = state_size;
@@ -128,7 +138,13 @@ void ONNXModel::execute() {
   if (desire_input_buf != NULL) {
     pwrite(desire_input_buf, desire_state_size);
   }
-  if (traffic_convention_input_buf != NULL) {
+  if (nav_features_input_buf != NULL) {
+    pwrite(nav_features_input_buf, nav_features_size);
+  }
+  if (driving_style_input_buf != NULL) {
+    pwrite(driving_style_input_buf, driving_style_size);
+  }
+   if (traffic_convention_input_buf != NULL) {
     pwrite(traffic_convention_input_buf, traffic_convention_size);
   }
   if (calib_input_buf != NULL) {

selfdrive/modeld/runners/onnxmodel.h

@@ -10,6 +10,8 @@ public:
 	~ONNXModel();
   void addRecurrent(float *state, int state_size);
   void addDesire(float *state, int state_size);
+  void addNavFeatures(float *state, int state_size);
+  void addDrivingStyle(float *state, int state_size);
   void addTrafficConvention(float *state, int state_size);
   void addCalib(float *state, int state_size);
   void addImage(float *image_buf, int buf_size);
@@ -25,6 +27,10 @@ private:
   int rnn_state_size;
   float *desire_input_buf = NULL;
   int desire_state_size;
+  float *nav_features_input_buf = NULL;
+  int nav_features_size;
+  float *driving_style_input_buf = NULL;
+  int driving_style_size;
   float *traffic_convention_input_buf = NULL;
   int traffic_convention_size;
   float *calib_input_buf = NULL;

selfdrive/modeld/runners/runmodel.h

@@ -5,6 +5,8 @@ public:
   virtual ~RunModel() {}
   virtual void addRecurrent(float *state, int state_size) {}
   virtual void addDesire(float *state, int state_size) {}
+  virtual void addNavFeatures(float *state, int state_size) {}
+  virtual void addDrivingStyle(float *state, int state_size) {}
   virtual void addTrafficConvention(float *state, int state_size) {}
   virtual void addCalib(float *state, int state_size) {}
   virtual void addImage(float *image_buf, int buf_size) {}

selfdrive/modeld/runners/snpemodel.cc

@@ -153,6 +153,16 @@ void SNPEModel::addDesire(float *state, int state_size) {
   desireBuffer = this->addExtra(state, state_size, 1);
 }
 
+void SNPEModel::addNavFeatures(float *state, int state_size) {
+  navFeatures = state;
+  navFeaturesBuffer = this->addExtra(state, state_size, 1);
+}
+
+void SNPEModel::addDrivingStyle(float *state, int state_size) {
+    drivingStyle = state;
+    drivingStyleBuffer = this->addExtra(state, state_size, 2);
+}
+
 void SNPEModel::addCalib(float *state, int state_size) {
   calib = state;
   calibBuffer = this->addExtra(state, state_size, 1);

selfdrive/modeld/runners/snpemodel.h

@@ -28,6 +28,8 @@ public:
   void addTrafficConvention(float *state, int state_size);
   void addCalib(float *state, int state_size);
   void addDesire(float *state, int state_size);
+  void addDrivingStyle(float *state, int state_size);
+  void addNavFeatures(float *state, int state_size);
   void addImage(float *image_buf, int buf_size);
   void addExtra(float *image_buf, int buf_size);
   void execute();
@@ -75,6 +77,10 @@ private:
   std::unique_ptr<zdl::DlSystem::IUserBuffer> trafficConventionBuffer;
   float *desire;
   std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
+  float *navFeatures;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> navFeaturesBuffer;
+  float *drivingStyle;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> drivingStyleBuffer;
   float *calib;
   std::unique_ptr<zdl::DlSystem::IUserBuffer> calibBuffer;
 };

selfdrive/modeld/runners/thneedmodel.cc

@@ -24,6 +24,14 @@ void ThneedModel::addDesire(float *state, int state_size) {
   desire = state;
 }
 
+void ThneedModel::addDrivingStyle(float *state, int state_size) {
+    drivingStyle = state;
+}
+
+void ThneedModel::addNavFeatures(float *state, int state_size) {
+  navFeatures = state;
+}
+
 void ThneedModel::addImage(float *image_input_buf, int buf_size) {
   input = image_input_buf;
 }

selfdrive/modeld/runners/thneedmodel.h

@@ -9,6 +9,8 @@ public:
   void addRecurrent(float *state, int state_size);
   void addTrafficConvention(float *state, int state_size);
   void addDesire(float *state, int state_size);
+  void addNavFeatures(float *state, int state_size);
+  void addDrivingStyle(float *state, int state_size);
   void addImage(float *image_buf, int buf_size);
   void addExtra(float *image_buf, int buf_size);
   void execute();
@@ -26,6 +28,8 @@ private:
   // recurrent and desire
   float *recurrent;
   float *trafficConvention;
+  float *drivingStyle;
   float *desire;
+  float *navFeatures;
 };
 

selfdrive/navd/map_renderer.cc

@@ -70,7 +70,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
 
   if (online) {
     vipc_server.reset(new VisionIpcServer("navd"));
-    vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT);
+    vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2);
     vipc_server->start_listener();
 
     pm.reset(new PubMaster({"navThumbnail", "mapRenderState"}));
@@ -177,10 +177,12 @@ void MapRenderer::publish(const double render_time) {
   uint8_t* dst = (uint8_t*)buf->addr;
   uint8_t* src = cap.bits();
 
-  // RGB to greyscale
+  // RGB to greyscale and crop
   memset(dst, 128, buf->len);
-  for (int i = 0; i < WIDTH * HEIGHT; i++) {
-    dst[i] = src[i * 3];
+  for (int r = 0; r < HEIGHT/2; r++) {
+    for (int c = 0; c < WIDTH/2; c++) {
+      dst[r*WIDTH/2 + c] = src[((HEIGHT/4 + r)*WIDTH + (c+WIDTH/4)) * 3];
+    }
   }
 
   vipc_server->send(buf, &extra);

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-30dfeed50ac562f99b0aad985bd6b0e6d8188fcb
+4ff972367fdb9546be68ee0ba0d45cf4f839dae7