diff --git a/release/files_common b/release/files_common index 3b84e09581..028b233d8b 100644 --- a/release/files_common +++ b/release/files_common @@ -361,11 +361,10 @@ selfdrive/modeld/.gitignore selfdrive/modeld/__init__.py selfdrive/modeld/SConscript selfdrive/modeld/modeld.py -selfdrive/modeld/navmodeld.cc +selfdrive/modeld/navmodeld.py selfdrive/modeld/dmonitoringmodeld.cc selfdrive/modeld/constants.py selfdrive/modeld/modeld -selfdrive/modeld/navmodeld selfdrive/modeld/dmonitoringmodeld selfdrive/modeld/models/__init__.py @@ -383,8 +382,6 @@ selfdrive/modeld/models/dmonitoring.cc selfdrive/modeld/models/dmonitoring.h 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 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 50e684fcb5..fd57cb9a8d 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -58,7 +58,7 @@ procs = [ NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad), NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad), - NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], only_onroad), + PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad), NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], only_onroad), diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 03836b6593..1c4d46795e 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -69,11 +69,6 @@ lenv.Program('_dmonitoringmodeld', [ "models/dmonitoring.cc", ]+common_model, LIBS=libs + snpe_lib) -lenv.Program('_navmodeld', [ - "navmodeld.cc", - "models/nav.cc", - ]+common_model, LIBS=libs + snpe_lib) - # Build thneed model if arch == "larch64" or GetOption('pc_thneed'): fn = File("models/supercombo").abspath diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index f13cd155f1..de0250d212 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -6,13 +6,16 @@ #include "cereal/messaging/messaging.h" #include "common/modeldata.h" #include "common/util.h" -#include "selfdrive/modeld/models/nav.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" constexpr int FEATURE_LEN = 128; 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 NAV_FEATURE_LEN = 256; +constexpr int NAV_INSTRUCTION_LEN = 150; constexpr int DRIVING_STYLE_LEN = 12; constexpr int MODEL_FREQ = 20; diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc deleted file mode 100644 index 8208b0bfef..0000000000 --- a/selfdrive/modeld/models/nav.cc +++ /dev/null @@ -1,71 +0,0 @@ -#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, true); - #else - s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, true); - #endif - - s->m->addInput("map", NULL, 0); -} - -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->setInputBuffer("map", (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 deleted file mode 100644 index 800abec252..0000000000 --- a/selfdrive/modeld/models/nav.h +++ /dev/null @@ -1,57 +0,0 @@ -#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 = 256; -constexpr int NAV_INSTRUCTION_LEN = 150; -constexpr int NAV_DESIRE_LEN = 32; - -struct NavModelOutputXY { - float x; - float y; -}; -static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2); - -struct NavModelOutputPlan { - std::array mean; - std::array std; -}; -static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2); - -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 NavModelOutputPlan plan; - const NavModelOutputDesirePrediction desire_pred; - const NavModelOutputFeatures features; - float dsp_execution_time; -}; -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; - -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, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid); -void navmodel_free(NavModelState* s); diff --git a/selfdrive/modeld/navmodeld b/selfdrive/modeld/navmodeld deleted file mode 100755 index 58215c2e9d..0000000000 --- a/selfdrive/modeld/navmodeld +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env bash - -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 deleted file mode 100644 index 4610f503d1..0000000000 --- a/selfdrive/modeld/navmodeld.cc +++ /dev/null @@ -1,70 +0,0 @@ -#include -#include - -#include -#include - -#include "cereal/visionipc/visionipc_client.h" -#include "common/params.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) { - SubMaster sm({"navInstruction"}); - 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; - - sm.update(0); - - 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, *model_res, (t2 - t1) / 1000.0, sm["navInstruction"].getValid()); - - //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); - - // there exists a race condition when two processes try to create a - // SNPE model runner at the same time, wait for dmonitoringmodeld to finish - LOGW("waiting for dmonitoringmodeld to initialize"); - if (!Params().getBool("DmModelInitialized", true)) { - return 0; - } - - // 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: %zu", vipc_client.buffers[0].len); - run_model(model, vipc_client); - } - - navmodel_free(&model); - return 0; -} diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py new file mode 100755 index 0000000000..54f85f9ae8 --- /dev/null +++ b/selfdrive/modeld/navmodeld.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 +import gc +import math +import time +import ctypes +import numpy as np +from pathlib import Path +from typing import Tuple, Dict + +from cereal import messaging +from cereal.messaging import PubMaster, SubMaster +from cereal.visionipc import VisionIpcClient, VisionStreamType +from openpilot.system.swaglog import cloudlog +from openpilot.common.params import Params +from openpilot.common.realtime import set_realtime_priority +from openpilot.selfdrive.modeld.constants import IDX_N +from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime + +NAV_INPUT_SIZE = 256*256 +NAV_FEATURE_LEN = 256 +NAV_DESIRE_LEN = 32 +NAV_OUTPUT_SIZE = 2*2*IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN +MODEL_PATHS = { + ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc', + ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'} + +class NavModelOutputXY(ctypes.Structure): + _fields_ = [ + ("x", ctypes.c_float), + ("y", ctypes.c_float)] + +class NavModelOutputPlan(ctypes.Structure): + _fields_ = [ + ("mean", NavModelOutputXY*IDX_N), + ("std", NavModelOutputXY*IDX_N)] + +class NavModelResult(ctypes.Structure): + _fields_ = [ + ("plan", NavModelOutputPlan), + ("desire_pred", ctypes.c_float*NAV_DESIRE_LEN), + ("features", ctypes.c_float*NAV_FEATURE_LEN)] + +class ModelState: + inputs: Dict[str, np.ndarray] + output: np.ndarray + model: ModelRunner + + def __init__(self): + assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float) + self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32) + self.inputs = {'map': np.zeros(NAV_INPUT_SIZE, dtype=np.uint8)} + self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None) + self.model.addInput("map", None) + + def run(self, buf:np.ndarray) -> Tuple[np.ndarray, float]: + self.inputs['map'][:] = buf + + t1 = time.perf_counter() + self.model.setInputBuffer("map", self.inputs['map'].view(np.float32)) + self.model.execute() + t2 = time.perf_counter() + return self.output, t2 - t1 + +def get_navmodel_packet(model_output: np.ndarray, valid: bool, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float): + model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(NavModelResult)).contents + msg = messaging.new_message('navModel') + msg.valid = valid + msg.navModel.frameId = frame_id + msg.navModel.locationMonoTime = location_ts + msg.navModel.modelExecutionTime = execution_time + msg.navModel.dspExecutionTime = dsp_execution_time + msg.navModel.features = model_result.features[:] + msg.navModel.desirePrediction = model_result.desire_pred[:] + msg.navModel.position.x = [p.x for p in model_result.plan.mean] + msg.navModel.position.y = [p.y for p in model_result.plan.mean] + msg.navModel.position.xStd = [math.exp(p.x) for p in model_result.plan.std] + msg.navModel.position.yStd = [math.exp(p.y) for p in model_result.plan.std] + return msg + + +def main(): + gc.disable() + set_realtime_priority(1) + + # there exists a race condition when two processes try to create a + # SNPE model runner at the same time, wait for dmonitoringmodeld to finish + cloudlog.warning("waiting for dmonitoringmodeld to initialize") + if not Params().get_bool("DmModelInitialized", True): + return + + model = ModelState() + cloudlog.warning("models loaded, navmodeld starting") + + vipc_client = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True) + while not vipc_client.connect(False): + time.sleep(0.1) + assert vipc_client.is_connected() + cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") + + sm = SubMaster(["navInstruction"]) + pm = PubMaster(["navModel"]) + + while True: + buf = vipc_client.recv() + if buf is None: + continue + + sm.update(0) + t1 = time.perf_counter() + model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset]) + t2 = time.perf_counter() + + valid = vipc_client.valid and sm.valid["navInstruction"] + pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time)) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 56bf9ea71d..9fa48b43d8 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -38,7 +38,7 @@ PROCS = { "selfdrive.controls.radard": 4.5, "selfdrive.modeld.modeld": 8.0, "./_dmonitoringmodeld": 5.0, - "./_navmodeld": 1.0, + "selfdrive.modeld.navmodeld": 1.0, "selfdrive.thermald.thermald": 3.87, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0,