Rewrite navmodeld in python (#29579)
* Added navmodeld.py
* Deleted navmodeld.cc
* Write SConscript config flags to config.py
* Remove deleted files from release/files_common
* Some more bug fixes
* Added config.py to gitignore
* Get rid of config.py
* Use set_realtime_priority
* A tiny bit more cleanup
* set realtime priority 1
* Use ModelRunner helper class from runners/__init__.py
* Formatting fixes
* mama mia that's a SPICY memory leak
old-commit-hash: 0c0af682a1
vw-mqb-aeb
Mitchell Goff
2 years ago
committed by
GitHub
parent
dc6078a6c3
commit
f9daa1d1cc
10 changed files with 125 additions and 222 deletions
@ -1,71 +0,0 @@ |
||||
#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, 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<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, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid) { |
||||
// make msg
|
||||
MessageBuilder msg; |
||||
auto evt = msg.initEvent(); |
||||
auto framed = evt.initNavModel(); |
||||
evt.setValid(extra.valid && route_valid); |
||||
framed.setFrameId(extra.frame_id); |
||||
framed.setLocationMonoTime(extra.timestamp_sof); |
||||
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.plan); |
||||
|
||||
pm.send("navModel", msg); |
||||
} |
||||
|
||||
void navmodel_free(NavModelState* s) { |
||||
delete s->m; |
||||
} |
||||
@ -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<NavModelOutputXY, TRAJECTORY_SIZE> mean; |
||||
std::array<NavModelOutputXY, TRAJECTORY_SIZE> std; |
||||
}; |
||||
static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2); |
||||
|
||||
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 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); |
||||
@ -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 |
||||
@ -1,70 +0,0 @@ |
||||
#include <sys/resource.h> |
||||
#include <limits.h> |
||||
|
||||
#include <cstdio> |
||||
#include <cstdlib> |
||||
|
||||
#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; |
||||
} |
||||
@ -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() |
||||
Loading…
Reference in new issue