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>

.gitignore

@@ -52,6 +52,7 @@ selfdrive/sensord/_sensord
 system/camerad/camerad
 system/camerad/test/ae_gray_test
 selfdrive/modeld/_modeld
+selfdrive/modeld/_navmodeld
 selfdrive/modeld/_dmonitoringmodeld
 /src/
 

release/files_common

@@ -350,20 +350,28 @@ selfdrive/manager/test/test_manager.py
 selfdrive/modeld/__init__.py
 selfdrive/modeld/SConscript
 selfdrive/modeld/modeld.cc
+selfdrive/modeld/navmodeld.cc
 selfdrive/modeld/dmonitoringmodeld.cc
 selfdrive/modeld/constants.py
 selfdrive/modeld/modeld
+selfdrive/modeld/navmodeld
 selfdrive/modeld/dmonitoringmodeld
 
 selfdrive/modeld/models/commonmodel.cc
 selfdrive/modeld/models/commonmodel.h
+
 selfdrive/modeld/models/driving.cc
 selfdrive/modeld/models/driving.h
+selfdrive/modeld/models/supercombo.onnx
+
 selfdrive/modeld/models/dmonitoring.cc
 selfdrive/modeld/models/dmonitoring.h
-selfdrive/modeld/models/supercombo.onnx
 selfdrive/modeld/models/dmonitoring_model_q.dlc
 
+selfdrive/modeld/models/nav.cc
+selfdrive/modeld/models/nav.h
+selfdrive/modeld/models/navmodel_q.dlc
+
 selfdrive/modeld/transforms/loadyuv.cc
 selfdrive/modeld/transforms/loadyuv.h
 selfdrive/modeld/transforms/loadyuv.cl

selfdrive/manager/process_config.py

@@ -32,7 +32,8 @@ procs = [
   NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]),
   NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging),
   NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
-  # NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"]),
+  NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False),
+  NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False),
   NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC),
   NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),

selfdrive/modeld/SConscript

@@ -112,3 +112,8 @@ llenv.Program('_modeld', [
     "modeld.cc",
     "models/driving.cc",
   ]+common_model, LIBS=libs + transformations)
+
+lenv.Program('_navmodeld', [
+    "navmodeld.cc",
+    "models/nav.cc",
+  ]+common_model, LIBS=libs + transformations)

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

selfdrive/modeld/navmodeld

@@ -0,0 +1,12 @@
+#!/bin/sh
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+cd $DIR
+
+if [ -f /TICI ]; then
+  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
+  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
+else
+  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
+fi
+exec ./_navmodeld

selfdrive/modeld/navmodeld.cc

@@ -0,0 +1,60 @@
+#include <sys/resource.h>
+#include <limits.h>
+
+#include <cstdio>
+#include <cstdlib>
+
+#include "cereal/visionipc/visionipc_client.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+#include "selfdrive/modeld/models/nav.h"
+
+ExitHandler do_exit;
+
+void run_model(NavModelState &model, VisionIpcClient &vipc_client) {
+  PubMaster pm({"navModel"});
+
+  double last_ts = 0;
+  uint32_t last_frame_id = 0;
+  VisionIpcBufExtra extra = {};
+
+  while (!do_exit) {
+    VisionBuf *buf = vipc_client.recv(&extra);
+    if (buf == nullptr) continue;
+    if (extra.frame_id < last_frame_id + 10) continue;  // Run at 2Hz
+
+    double t1 = millis_since_boot();
+    NavModelResult *model_res = navmodel_eval_frame(&model, buf);
+    double t2 = millis_since_boot();
+
+    // send navmodel packet
+    navmodel_publish(pm, extra.frame_id, *model_res, (t2 - t1) / 1000.0);
+
+    //printf("navmodel process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last_ts);
+    last_ts = t1;
+    last_frame_id = extra.frame_id;
+  }
+}
+
+int main(int argc, char **argv) {
+  setpriority(PRIO_PROCESS, 0, -15);
+
+  // init the models
+  NavModelState model;
+  navmodel_init(&model);
+  LOGW("models loaded, navmodeld starting");
+
+  VisionIpcClient vipc_client = VisionIpcClient("navd", VISION_STREAM_MAP, true);
+  while (!do_exit && !vipc_client.connect(false)) {
+    util::sleep_for(100);
+  }
+
+  // run the models
+  if (vipc_client.connected) {
+    LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
+    run_model(model, vipc_client);
+  }
+
+  navmodel_free(&model);
+  return 0;
+}