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 leakpull/29793/head
Mitchell Goff
2 years ago
committed by
GitHub
parent
73eda51a11
commit
0c0af682a1
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