From 46f70592ae6239796f673bd5e9bfef13d1efeb9c Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Fri, 2 Dec 2022 13:14:30 -0800 Subject: [PATCH] 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 old-commit-hash: bb8a38a0508c764c8340e7b16968b06a8367ab7a --- .gitignore | 1 + release/files_common | 10 +++- selfdrive/manager/process_config.py | 3 +- selfdrive/modeld/SConscript | 5 ++ selfdrive/modeld/models/commonmodel.h | 6 +++ selfdrive/modeld/models/driving.cc | 5 -- selfdrive/modeld/models/nav.cc | 66 +++++++++++++++++++++++ selfdrive/modeld/models/nav.h | 73 ++++++++++++++++++++++++++ selfdrive/modeld/models/navmodel.onnx | 3 ++ selfdrive/modeld/models/navmodel_q.dlc | 3 ++ selfdrive/modeld/navmodeld | 12 +++++ selfdrive/modeld/navmodeld.cc | 60 +++++++++++++++++++++ 12 files changed, 240 insertions(+), 7 deletions(-) create mode 100644 selfdrive/modeld/models/nav.cc create mode 100644 selfdrive/modeld/models/nav.h create mode 100644 selfdrive/modeld/models/navmodel.onnx create mode 100644 selfdrive/modeld/models/navmodel_q.dlc create mode 100755 selfdrive/modeld/navmodeld create mode 100644 selfdrive/modeld/navmodeld.cc diff --git a/.gitignore b/.gitignore index 31cef9422..2b283d3b1 100644 --- a/.gitignore +++ b/.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/ diff --git a/release/files_common b/release/files_common index 2f7f2d4d6..297a7a808 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 50c19610e..c03e99549 100644 --- a/selfdrive/manager/process_config.py +++ b/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)), diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index f1a8d7188..82338e456 100644 --- a/selfdrive/modeld/SConscript +++ b/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) \ No newline at end of file diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 40c82a8c2..1a079da05 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/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 +constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { + return kj::ArrayPtr(arr.data(), arr.size()); +} + class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 4015731c4..9cb216ff1 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -22,11 +22,6 @@ std::array prev_brake_3ms2_probs = {0,0,0}; // #define DUMP_YUV -template -constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &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); diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc new file mode 100644 index 000000000..dae87c7ce --- /dev/null +++ b/selfdrive/modeld/models/nav.cc @@ -0,0 +1,66 @@ +#include "selfdrive/modeld/models/nav.h" + +#include +#include + +#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 pos_x, pos_y; + std::array pos_x_std, pos_y_std; + + for (int i=0; im; +} diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h new file mode 100644 index 000000000..b469f7598 --- /dev/null +++ b/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 mean; + std::array std; + float prob; +}; +static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2 + sizeof(float)); + +struct NavModelOutputPlans { + std::array 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 values; +}; +static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN); + +struct NavModelOutputFeatures { + std::array 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); diff --git a/selfdrive/modeld/models/navmodel.onnx b/selfdrive/modeld/models/navmodel.onnx new file mode 100644 index 000000000..60dd8c0e7 --- /dev/null +++ b/selfdrive/modeld/models/navmodel.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eab4b986e14d7d842d6d5487011c329d356fb56995b2ae7dc7188aefe6df9d97 +size 12285002 diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc new file mode 100644 index 000000000..7d9b36ed4 --- /dev/null +++ b/selfdrive/modeld/models/navmodel_q.dlc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:83d53efc40053b02fe7d3da4ef6213a4a5a1ae4d1bd49c121b9beb6a54ea1148 +size 3154868 diff --git a/selfdrive/modeld/navmodeld b/selfdrive/modeld/navmodeld new file mode 100755 index 000000000..079afd967 --- /dev/null +++ b/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 diff --git a/selfdrive/modeld/navmodeld.cc b/selfdrive/modeld/navmodeld.cc new file mode 100644 index 000000000..57ef7cf0e --- /dev/null +++ b/selfdrive/modeld/navmodeld.cc @@ -0,0 +1,60 @@ +#include +#include + +#include +#include + +#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; +}