navmodeld (#26665)

* Added navmodeld

* New nav model: 7c306685-5476-4bd4-ab65-105b01b6bca8/300, feats only

* little cleanup

* Remove NAV flag

* Moved to_kj_array_ptr to commonmodel.h

* Switch from decimation to last_frame_id check

* add to release files

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: bb8a38a0508c764c8340e7b16968b06a8367ab7a

selfdrive/modeld/models/commonmodel.h

@@ -13,6 +13,7 @@
 #endif
 
 #include "common/mat.h"
+#include "cereal/messaging/messaging.h"
 #include "selfdrive/modeld/transforms/loadyuv.h"
 #include "selfdrive/modeld/transforms/transform.h"
 
@@ -21,6 +22,11 @@ const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
 void softmax(const float* input, float* output, size_t len);
 float sigmoid(float input);
 
+template<class T, size_t size>
+constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
+  return kj::ArrayPtr(arr.data(), arr.size());
+}
+
 class ModelFrame {
 public:
   ModelFrame(cl_device_id device_id, cl_context context);

selfdrive/modeld/models/driving.cc

@@ -22,11 +22,6 @@ std::array<float, 3> prev_brake_3ms2_probs = {0,0,0};
 
 // #define DUMP_YUV
 
-template<class T, size_t size>
-constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
-  return kj::ArrayPtr(arr.data(), arr.size());
-}
-
 void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   s->frame = new ModelFrame(device_id, context);
   s->wide_frame = new ModelFrame(device_id, context);

selfdrive/modeld/models/nav.cc

@@ -0,0 +1,66 @@
+#include "selfdrive/modeld/models/nav.h"
+
+#include <cstdio>
+#include <cstring>
+
+#include "common/mat.h"
+#include "common/modeldata.h"
+#include "common/timing.h"
+
+
+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
+}
+
+NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) {
+  memcpy(s->net_input_buf, buf->addr, NAV_INPUT_SIZE);
+
+  double t1 = millis_since_boot();
+  s->m->addImage((float*)s->net_input_buf, NAV_INPUT_SIZE/sizeof(float));
+  s->m->execute();
+  double t2 = millis_since_boot();
+
+  NavModelResult *model_res = (NavModelResult*)&s->output;
+  model_res->dsp_execution_time = (t2 - t1) / 1000.;
+  return model_res;
+}
+
+void fill_plan(cereal::NavModelData::Builder &framed, const NavModelOutputPlan &plan) {
+  std::array<float, TRAJECTORY_SIZE> pos_x, pos_y;
+  std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std;
+
+  for (int i=0; i<TRAJECTORY_SIZE; i++) {
+    pos_x[i] = plan.mean[i].x;
+    pos_y[i] = plan.mean[i].y;
+    pos_x_std[i] = exp(plan.std[i].x);
+    pos_y_std[i] = exp(plan.std[i].y);
+  }
+
+  auto position = framed.initPosition();
+  position.setX(to_kj_array_ptr(pos_x));
+  position.setY(to_kj_array_ptr(pos_y));
+  position.setXStd(to_kj_array_ptr(pos_x_std));
+  position.setYStd(to_kj_array_ptr(pos_y_std));
+}
+
+void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time) {
+  // make msg
+  MessageBuilder msg;
+  auto framed = msg.initEvent().initNavModel();
+  framed.setFrameId(frame_id);
+  framed.setModelExecutionTime(execution_time);
+  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());
+
+  pm.send("navModel", msg);
+}
+
+void navmodel_free(NavModelState* s) {
+  delete s->m;
+}

selfdrive/modeld/models/nav.h

@@ -0,0 +1,73 @@
+#pragma once
+
+#include "cereal/messaging/messaging.h"
+#include "cereal/visionipc/visionipc_client.h"
+#include "common/util.h"
+#include "common/modeldata.h"
+#include "selfdrive/modeld/models/commonmodel.h"
+#include "selfdrive/modeld/runners/run.h"
+
+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;
+  float y;
+};
+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);
+
+struct NavModelOutputDesirePrediction {
+  std::array<float, NAV_DESIRE_LEN> values;
+};
+static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN);
+
+struct NavModelOutputFeatures {
+  std::array<float, NAV_FEATURE_LEN> values;
+};
+static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
+
+struct NavModelResult {
+  const NavModelOutputPlans plans;
+  const NavModelOutputDesirePrediction desire_pred;
+  const NavModelOutputFeatures features;
+  float dsp_execution_time;
+};
+static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlans) + 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;
+
+struct NavModelState {
+  RunModel *m;
+  uint8_t net_input_buf[NAV_INPUT_SIZE];
+  float output[NAV_OUTPUT_SIZE];
+};
+
+void navmodel_init(NavModelState* s);
+NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf);
+void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time);
+void navmodel_free(NavModelState* s);

selfdrive/modeld/models/navmodel.onnx

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

selfdrive/modeld/models/navmodel_q.dlc

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