mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rewrite navmodeld in python (#29579)

Browse Source 
* 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: 0c0af682a13753a3db971b12c2852cfddf1e5b49
This commit is contained in:
Mitchell Goff
2023-09-07 14:39:37 -07:00
committed by GitHub
parent dc6078a6c3
commit f9daa1d1cc
10 changed files with 125 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
release/files_common
+1 -4
View File
@@ -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
selfdrive/manager/process_config.py
+1 -1
View File
@@ -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),
selfdrive/modeld/SConscript
-5
View File
@@ -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
selfdrive/modeld/models/driving.h
+4 -1
View File
@@ -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;
selfdrive/modeld/models/nav.cc
-71
View File
@@ -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;
}
selfdrive/modeld/models/nav.h
-57
View File
@@ -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);
selfdrive/modeld/navmodeld
-12
View File
@@ -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
selfdrive/modeld/navmodeld.cc
-70
View File
@@ -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;
}
selfdrive/modeld/navmodeld.py
Executable
+118
View File
@@ -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()
selfdrive/test/test_onroad.py
+1 -1
View File
@@ -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,