Rewrite modeld in python (#29230)
* Added modeld.py (WIP) * No more VisionIpcBufExtra * Started work on cython bindings for runmodel * Got ONNXModel cython bindings mostly working, added ModelFrame bindings * Got modeld main loop running without model eval * Move everything into ModelState * Doesn't crash! * Moved ModelState into modeld.py * Added driving_pyx * Added cython bindings for message generation * Moved CLContext definition to visionipc.pxd * *facepalm* * Move cl_pyx into commonmodel_pyx * Split out ONNXModel into a subclass of RunModel * Added snpemodel/thneedmodel bindings * Removed modeld.cc * Fixed scons for macOS * Fixed sconscript * Added flag for thneedmodel * paths are now relative to openpilot root dir * Set cl kernel paths in SConscript * Set LD_PRELOAD=libthneed.so to fix ioctl interception * Run from root dir * A few more fixes * A few more minor fixes * Use C update_calibration for now to exactly match refs * Add nav_instructions input * Link driving_pyx.pyx with transformations * Checked python FirstOrderFilter against C++ FirstOrderFilter * Set process name to fix test_onroad * Revert changes to onnxmodel.cc * Fixed bad onnx_runner.py path in onnxmodel.cc * Import all constants from driving.h * logging -> cloudlog * pylint import-error suppressions no longer needed? * Loop in SConscript * Added parens * Bump modeld cpu usage in test_onroad * Get rid of use_nav * use config_realtime_process * error message from ioctl sniffer was messing up pyenv * cast distance_idx to int * Removed cloudlog.infos in model.run * Fixed rebase conflicts * Clean up driving.pxd/pyx * Fixed linter errorpull/29636/head
Mitchell Goff
2 years ago
committed by
GitHub
parent
c6c6877263
commit
72a3c987c0
10 changed files with 452 additions and 480 deletions
@ -1,11 +1,7 @@ |
||||
#!/bin/sh |
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" |
||||
cd $DIR |
||||
cd "$DIR/../../" |
||||
|
||||
if [ -f /TICI ]; then |
||||
export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH" |
||||
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 ./_modeld |
||||
export LD_PRELOAD="$DIR/libthneed.so" |
||||
exec "$DIR/modeld.py" |
||||
|
||||
@ -1,271 +0,0 @@ |
||||
#include <cstdio> |
||||
#include <cstdlib> |
||||
#include <mutex> |
||||
#include <cmath> |
||||
|
||||
#include <eigen3/Eigen/Dense> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "common/transformations/orientation.hpp" |
||||
|
||||
#include "cereal/visionipc/visionipc_client.h" |
||||
#include "common/clutil.h" |
||||
#include "common/params.h" |
||||
#include "common/swaglog.h" |
||||
#include "common/util.h" |
||||
#include "system/hardware/hw.h" |
||||
#include "selfdrive/modeld/models/driving.h" |
||||
#include "selfdrive/modeld/models/nav.h" |
||||
|
||||
|
||||
ExitHandler do_exit; |
||||
|
||||
mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camera, bool bigmodel_frame) { |
||||
/*
|
||||
import numpy as np |
||||
from openpilot.common.transformations.model import medmodel_frame_from_calib_frame |
||||
medmodel_frame_from_calib_frame = medmodel_frame_from_calib_frame[:, :3] |
||||
calib_from_smedmodel_frame = np.linalg.inv(medmodel_frame_from_calib_frame) |
||||
*/ |
||||
static const auto calib_from_medmodel = (Eigen::Matrix<float, 3, 3>() << |
||||
0.00000000e+00, 0.00000000e+00, 1.00000000e+00, |
||||
1.09890110e-03, 0.00000000e+00, -2.81318681e-01, |
||||
-2.25466395e-20, 1.09890110e-03, -5.23076923e-02).finished(); |
||||
|
||||
static const auto calib_from_sbigmodel = (Eigen::Matrix<float, 3, 3>() << |
||||
0.00000000e+00, 7.31372216e-19, 1.00000000e+00, |
||||
2.19780220e-03, 4.11497335e-19, -5.62637363e-01, |
||||
-6.66298828e-20, 2.19780220e-03, -3.33626374e-01).finished(); |
||||
|
||||
static const auto view_from_device = (Eigen::Matrix<float, 3, 3>() << |
||||
0.0, 1.0, 0.0, |
||||
0.0, 0.0, 1.0, |
||||
1.0, 0.0, 0.0).finished(); |
||||
|
||||
|
||||
const auto cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(wide_camera ? ECAM_INTRINSIC_MATRIX.v : FCAM_INTRINSIC_MATRIX.v); |
||||
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> device_from_calib = euler2rot(device_from_calib_euler).cast <float> (); |
||||
auto calib_from_model = bigmodel_frame ? calib_from_sbigmodel : calib_from_medmodel; |
||||
auto camera_from_calib = cam_intrinsics * view_from_device * device_from_calib; |
||||
auto warp_matrix = camera_from_calib * calib_from_model; |
||||
|
||||
mat3 transform = {}; |
||||
for (int i=0; i<3*3; i++) { |
||||
transform.v[i] = warp_matrix(i / 3, i % 3); |
||||
} |
||||
return transform; |
||||
} |
||||
|
||||
|
||||
void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) { |
||||
// messaging
|
||||
PubMaster pm({"modelV2", "cameraOdometry"}); |
||||
SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"}); |
||||
|
||||
Params params; |
||||
PublishState ps = {}; |
||||
|
||||
// setup filter to track dropped frames
|
||||
FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); |
||||
|
||||
uint32_t frame_id = 0, last_vipc_frame_id = 0; |
||||
// double last = 0;
|
||||
uint32_t run_count = 0; |
||||
|
||||
mat3 model_transform_main = {}; |
||||
mat3 model_transform_extra = {}; |
||||
bool nav_enabled = false; |
||||
bool live_calib_seen = false; |
||||
float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}; |
||||
float nav_features[NAV_FEATURE_LEN] = {0}; |
||||
float nav_instructions[NAV_INSTRUCTION_LEN] = {0}; |
||||
|
||||
VisionBuf *buf_main = nullptr; |
||||
VisionBuf *buf_extra = nullptr; |
||||
|
||||
VisionIpcBufExtra meta_main = {0}; |
||||
VisionIpcBufExtra meta_extra = {0}; |
||||
|
||||
while (!do_exit) { |
||||
// Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) { |
||||
buf_main = vipc_client_main.recv(&meta_main); |
||||
if (buf_main == nullptr) break; |
||||
} |
||||
|
||||
if (buf_main == nullptr) { |
||||
LOGE("vipc_client_main no frame"); |
||||
continue; |
||||
} |
||||
|
||||
if (use_extra_client) { |
||||
// Keep receiving extra frames until frame id matches main camera
|
||||
do { |
||||
buf_extra = vipc_client_extra.recv(&meta_extra); |
||||
} while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL); |
||||
|
||||
if (buf_extra == nullptr) { |
||||
LOGE("vipc_client_extra no frame"); |
||||
continue; |
||||
} |
||||
|
||||
if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) { |
||||
LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)", |
||||
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9, |
||||
meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9); |
||||
} |
||||
} else { |
||||
// Use single camera
|
||||
buf_extra = buf_main; |
||||
meta_extra = meta_main; |
||||
} |
||||
|
||||
// TODO: path planner timeout?
|
||||
sm.update(0); |
||||
int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); |
||||
bool is_rhd = ((bool)sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); |
||||
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); |
||||
if (sm.updated("liveCalibration")) { |
||||
auto rpy_calib = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); |
||||
Eigen::Vector3d device_from_calib_euler; |
||||
for (int i=0; i<3; i++) { |
||||
device_from_calib_euler(i) = rpy_calib[i]; |
||||
} |
||||
model_transform_main = update_calibration(device_from_calib_euler, main_wide_camera, false); |
||||
model_transform_extra = update_calibration(device_from_calib_euler, true, true); |
||||
live_calib_seen = true; |
||||
} |
||||
|
||||
float vec_desire[DESIRE_LEN] = {0}; |
||||
if (desire >= 0 && desire < DESIRE_LEN) { |
||||
vec_desire[desire] = 1.0; |
||||
} |
||||
|
||||
// Enable/disable nav features
|
||||
uint64_t timestamp_llk = sm["navModel"].getNavModel().getLocationMonoTime(); |
||||
bool nav_valid = sm["navModel"].getValid() && (nanos_since_boot() - timestamp_llk < 1e9); |
||||
bool use_nav = nav_valid && params.getBool("ExperimentalMode"); |
||||
if (!nav_enabled && use_nav) { |
||||
nav_enabled = true; |
||||
} else if (nav_enabled && !use_nav) { |
||||
memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN); |
||||
memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN); |
||||
nav_enabled = false; |
||||
} |
||||
|
||||
if (nav_enabled && sm.updated("navModel")) { |
||||
auto nav_model_features = sm["navModel"].getNavModel().getFeatures(); |
||||
for (int i=0; i<NAV_FEATURE_LEN; i++) { |
||||
nav_features[i] = nav_model_features[i]; |
||||
} |
||||
} |
||||
|
||||
if (nav_enabled && sm.updated("navInstruction")) { |
||||
memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN); |
||||
auto maneuvers = sm["navInstruction"].getNavInstruction().getAllManeuvers(); |
||||
for (int i=0; i<maneuvers.size(); i++) { |
||||
int distance_idx = 25 + (int)(maneuvers[i].getDistance() / 20); |
||||
std::string direction = maneuvers[i].getModifier(); |
||||
int direction_idx = 0; |
||||
if (direction == "left" || direction == "slight left" || direction == "sharp left") direction_idx = 1; |
||||
if (direction == "right" || direction == "slight right" || direction == "sharp right") direction_idx = 2; |
||||
if (distance_idx >= 0 && distance_idx < 50) { |
||||
nav_instructions[distance_idx*3 + direction_idx] = 1; |
||||
} |
||||
} |
||||
} |
||||
|
||||
// tracked dropped frames
|
||||
uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1; |
||||
float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U)); |
||||
if (run_count < 10) { // let frame drops warm up
|
||||
frame_dropped_filter.reset(0); |
||||
frames_dropped = 0.; |
||||
} |
||||
run_count++; |
||||
|
||||
float frame_drop_ratio = frames_dropped / (1 + frames_dropped); |
||||
bool prepare_only = vipc_dropped_frames > 0; |
||||
|
||||
if (prepare_only) { |
||||
LOGE("skipping model eval. Dropped %d frames", vipc_dropped_frames); |
||||
} |
||||
|
||||
double mt1 = millis_since_boot(); |
||||
ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, nav_instructions, prepare_only); |
||||
double mt2 = millis_since_boot(); |
||||
float model_execution_time = (mt2 - mt1) / 1000.0; |
||||
|
||||
if (model_output != nullptr) { |
||||
model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, model, ps, meta_main.timestamp_eof, timestamp_llk, model_execution_time, |
||||
nav_enabled, live_calib_seen); |
||||
posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen); |
||||
} |
||||
|
||||
// printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
|
||||
// last = mt1;
|
||||
last_vipc_frame_id = meta_main.frame_id; |
||||
} |
||||
} |
||||
|
||||
int main(int argc, char **argv) { |
||||
if (!Hardware::PC()) { |
||||
int ret; |
||||
ret = util::set_realtime_priority(54); |
||||
assert(ret == 0); |
||||
util::set_core_affinity({7}); |
||||
assert(ret == 0); |
||||
} |
||||
|
||||
// cl init
|
||||
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); |
||||
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); |
||||
|
||||
// init the models
|
||||
ModelState model; |
||||
model_init(&model, device_id, context); |
||||
LOGW("models loaded, modeld starting"); |
||||
|
||||
bool main_wide_camera = false; |
||||
bool use_extra_client = true; // set to false to use single camera
|
||||
while (!do_exit) { |
||||
auto streams = VisionIpcClient::getAvailableStreams("camerad", false); |
||||
if (!streams.empty()) { |
||||
use_extra_client = streams.count(VISION_STREAM_WIDE_ROAD) > 0 && streams.count(VISION_STREAM_ROAD) > 0; |
||||
main_wide_camera = streams.count(VISION_STREAM_ROAD) == 0; |
||||
break; |
||||
} |
||||
|
||||
util::sleep_for(100); |
||||
} |
||||
|
||||
VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context); |
||||
VisionIpcClient vipc_client_extra = VisionIpcClient("camerad", VISION_STREAM_WIDE_ROAD, false, device_id, context); |
||||
LOGW("vision stream set up, main_wide_camera: %d, use_extra_client: %d", main_wide_camera, use_extra_client); |
||||
|
||||
while (!do_exit && !vipc_client_main.connect(false)) { |
||||
util::sleep_for(100); |
||||
} |
||||
|
||||
while (!do_exit && use_extra_client && !vipc_client_extra.connect(false)) { |
||||
util::sleep_for(100); |
||||
} |
||||
|
||||
// run the models
|
||||
// vipc_client.connected is false only when do_exit is true
|
||||
if (!do_exit) { |
||||
const VisionBuf *b = &vipc_client_main.buffers[0]; |
||||
LOGW("connected main cam with buffer size: %zu (%zu x %zu)", b->len, b->width, b->height); |
||||
|
||||
if (use_extra_client) { |
||||
const VisionBuf *wb = &vipc_client_extra.buffers[0]; |
||||
LOGW("connected extra cam with buffer size: %zu (%zu x %zu)", wb->len, wb->width, wb->height); |
||||
} |
||||
|
||||
run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra_client); |
||||
} |
||||
|
||||
model_free(&model); |
||||
CL_CHECK(clReleaseContext(context)); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,279 @@ |
||||
#!/usr/bin/env python3 |
||||
import sys |
||||
import time |
||||
import numpy as np |
||||
from pathlib import Path |
||||
from typing import Dict, Optional |
||||
from setproctitle import setproctitle |
||||
from cereal.messaging import PubMaster, SubMaster |
||||
from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf |
||||
from openpilot.system.hardware import PC |
||||
from openpilot.system.swaglog import cloudlog |
||||
from openpilot.common.params import Params |
||||
from openpilot.common.filter_simple import FirstOrderFilter |
||||
from openpilot.common.realtime import config_realtime_process |
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext, Runtime |
||||
from openpilot.selfdrive.modeld.models.driving_pyx import ( |
||||
PublishState, create_model_msg, create_pose_msg, update_calibration, |
||||
FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN, |
||||
OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ, USE_THNEED) |
||||
|
||||
if USE_THNEED: |
||||
from selfdrive.modeld.runners.thneedmodel_pyx import ThneedModel as ModelRunner |
||||
else: |
||||
from selfdrive.modeld.runners.onnxmodel_pyx import ONNXModel as ModelRunner |
||||
|
||||
MODEL_PATH = str(Path(__file__).parent / f"models/supercombo.{'thneed' if USE_THNEED else 'onnx'}") |
||||
|
||||
# NOTE: numpy matmuls don't seem to perfectly match eigen matmuls so the ref test fails, but we should switch to the np version after checking compare_runtime |
||||
# from common.transformations.orientation import rot_from_euler |
||||
# from common.transformations.model import medmodel_frame_from_calib_frame, sbigmodel_frame_from_calib_frame |
||||
# from common.transformations.camera import view_frame_from_device_frame, tici_fcam_intrinsics, tici_ecam_intrinsics |
||||
# calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3]) |
||||
# calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3]) |
||||
# |
||||
# def update_calibration(device_from_calib_euler: np.ndarray, wide_camera: bool, bigmodel_frame: bool) -> np.ndarray: |
||||
# cam_intrinsics = tici_ecam_intrinsics if wide_camera else tici_fcam_intrinsics |
||||
# calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel |
||||
# device_from_calib = rot_from_euler(device_from_calib_euler) |
||||
# camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib |
||||
# warp_matrix: np.ndarray = camera_from_calib @ calib_from_model |
||||
# return warp_matrix |
||||
|
||||
class FrameMeta: |
||||
frame_id: int = 0 |
||||
timestamp_sof: int = 0 |
||||
timestamp_eof: int = 0 |
||||
|
||||
def __init__(self, vipc=None): |
||||
if vipc is not None: |
||||
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof |
||||
|
||||
class ModelState: |
||||
frame: ModelFrame |
||||
wide_frame: ModelFrame |
||||
inputs: Dict[str, np.ndarray] |
||||
output: np.ndarray |
||||
prev_desire: np.ndarray # for tracking the rising edge of the pulse |
||||
model: ModelRunner |
||||
|
||||
def __init__(self, context: CLContext): |
||||
self.frame = ModelFrame(context) |
||||
self.wide_frame = ModelFrame(context) |
||||
self.prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32) |
||||
self.output = np.zeros(NET_OUTPUT_SIZE, dtype=np.float32) |
||||
self.inputs = { |
||||
'desire_pulse': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32), |
||||
'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32), |
||||
'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32), |
||||
'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32), |
||||
'feature_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32), |
||||
} |
||||
|
||||
self.model = ModelRunner(MODEL_PATH, self.output, Runtime.GPU, False, context) |
||||
self.model.addInput("input_imgs", None) |
||||
self.model.addInput("big_input_imgs", None) |
||||
for k,v in self.inputs.items(): |
||||
self.model.addInput(k, v) |
||||
|
||||
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, |
||||
inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[np.ndarray]: |
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge |
||||
inputs['desire_pulse'][0] = 0 |
||||
self.inputs['desire_pulse'][:-DESIRE_LEN] = self.inputs['desire_pulse'][DESIRE_LEN:] |
||||
self.inputs['desire_pulse'][-DESIRE_LEN:] = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) |
||||
self.prev_desire[:] = inputs['desire_pulse'] |
||||
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention'] |
||||
self.inputs['nav_features'][:] = inputs['nav_features'] |
||||
self.inputs['nav_instructions'][:] = inputs['nav_instructions'] |
||||
# self.inputs['driving_style'][:] = inputs['driving_style'] |
||||
|
||||
# if getCLBuffer is not None, frame will be None |
||||
self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) |
||||
if wbuf is not None: |
||||
self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))) |
||||
|
||||
if prepare_only: |
||||
return None |
||||
|
||||
self.model.execute() |
||||
self.inputs['feature_buffer'][:-FEATURE_LEN] = self.inputs['feature_buffer'][FEATURE_LEN:] |
||||
self.inputs['feature_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN] |
||||
return self.output |
||||
|
||||
|
||||
def main(): |
||||
cloudlog.bind(daemon="selfdrive.modeld.modeld") |
||||
setproctitle("selfdrive.modeld.modeld") |
||||
if not PC: |
||||
config_realtime_process(7, 54) |
||||
|
||||
cl_context = CLContext() |
||||
model = ModelState(cl_context) |
||||
cloudlog.warning("models loaded, modeld starting") |
||||
|
||||
# visionipc clients |
||||
while True: |
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False) |
||||
if available_streams: |
||||
use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams |
||||
main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams |
||||
break |
||||
time.sleep(.1) |
||||
|
||||
vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD |
||||
vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context) |
||||
vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) |
||||
cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") |
||||
|
||||
while not vipc_client_main.connect(False): |
||||
time.sleep(0.1) |
||||
while not vipc_client_extra.connect(False): |
||||
time.sleep(0.1) |
||||
|
||||
cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})") |
||||
if use_extra_client: |
||||
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") |
||||
|
||||
# messaging |
||||
pm = PubMaster(["modelV2", "cameraOdometry"]) |
||||
sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"]) |
||||
|
||||
state = PublishState() |
||||
params = Params() |
||||
|
||||
# setup filter to track dropped frames |
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / MODEL_FREQ) |
||||
frame_id = 0 |
||||
last_vipc_frame_id = 0 |
||||
run_count = 0 |
||||
# last = 0.0 |
||||
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32) |
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32) |
||||
live_calib_seen = False |
||||
driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) |
||||
nav_features = np.zeros(NAV_FEATURE_LEN, dtype=np.float32) |
||||
nav_instructions = np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32) |
||||
buf_main, buf_extra = None, None |
||||
meta_main = FrameMeta() |
||||
meta_extra = FrameMeta() |
||||
|
||||
while True: |
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame |
||||
while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: |
||||
buf_main = vipc_client_main.recv() |
||||
meta_main = FrameMeta(vipc_client_main) |
||||
if buf_main is None: |
||||
break |
||||
|
||||
if buf_main is None: |
||||
cloudlog.error("vipc_client_main no frame") |
||||
continue |
||||
|
||||
if use_extra_client: |
||||
# Keep receiving extra frames until frame id matches main camera |
||||
while True: |
||||
buf_extra = vipc_client_extra.recv() |
||||
meta_extra = FrameMeta(vipc_client_extra) |
||||
if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: |
||||
break |
||||
|
||||
if buf_extra is None: |
||||
cloudlog.error("vipc_client_extra no frame") |
||||
continue |
||||
|
||||
if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: |
||||
cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format( |
||||
meta_main.frame_id, meta_main.timestamp_sof / 1e9, |
||||
meta_extra.frame_id, meta_extra.timestamp_sof / 1e9)) |
||||
|
||||
else: |
||||
# Use single camera |
||||
buf_extra = buf_main |
||||
meta_extra = meta_main |
||||
|
||||
# TODO: path planner timeout? |
||||
sm.update(0) |
||||
desire = sm["lateralPlan"].desire.raw |
||||
is_rhd = sm["driverMonitoringState"].isRHD |
||||
frame_id = sm["roadCameraState"].frameId |
||||
if sm.updated["liveCalibration"]: |
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) |
||||
model_transform_main = update_calibration(device_from_calib_euler, main_wide_camera, False) |
||||
model_transform_extra = update_calibration(device_from_calib_euler, True, True) |
||||
live_calib_seen = True |
||||
|
||||
traffic_convention = np.zeros(2) |
||||
traffic_convention[int(is_rhd)] = 1 |
||||
|
||||
vec_desire = np.zeros(DESIRE_LEN, dtype=np.float32) |
||||
if desire >= 0 and desire < DESIRE_LEN: |
||||
vec_desire[desire] = 1 |
||||
|
||||
# Enable/disable nav features |
||||
timestamp_llk = sm["navModel"].locationMonoTime |
||||
nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9) |
||||
nav_enabled = nav_valid and params.get_bool("ExperimentalMode") |
||||
|
||||
if not nav_enabled: |
||||
nav_features[:] = 0 |
||||
nav_instructions[:] = 0 |
||||
|
||||
if nav_enabled and sm.updated["navModel"]: |
||||
nav_features = np.array(sm["navModel"].features) |
||||
|
||||
if nav_enabled and sm.updated["navInstruction"]: |
||||
nav_instructions[:] = 0 |
||||
for maneuver in sm["navInstruction"].allManeuvers: |
||||
distance_idx = 25 + int(maneuver.distance / 20) |
||||
direction_idx = 0 |
||||
if maneuver.modifier in ("left", "slight left", "sharp left"): |
||||
direction_idx = 1 |
||||
if maneuver.modifier in ("right", "slight right", "sharp right"): |
||||
direction_idx = 2 |
||||
if 0 <= distance_idx < 50: |
||||
nav_instructions[distance_idx*3 + direction_idx] = 1 |
||||
|
||||
# tracked dropped frames |
||||
vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) |
||||
frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) |
||||
if run_count < 10: # let frame drops warm up |
||||
frame_dropped_filter.x = 0. |
||||
frames_dropped = 0. |
||||
run_count = run_count + 1 |
||||
|
||||
frame_drop_ratio = frames_dropped / (1 + frames_dropped) |
||||
prepare_only = vipc_dropped_frames > 0 |
||||
if prepare_only: |
||||
cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") |
||||
|
||||
inputs:Dict[str, np.ndarray] = { |
||||
'desire_pulse': vec_desire, |
||||
'traffic_convention': traffic_convention, |
||||
'driving_style': driving_style, |
||||
'nav_features': nav_features, |
||||
'nav_instructions': nav_instructions} |
||||
|
||||
mt1 = time.perf_counter() |
||||
model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) |
||||
mt2 = time.perf_counter() |
||||
model_execution_time = mt2 - mt1 |
||||
|
||||
if model_output is not None: |
||||
pm.send("modelV2", create_model_msg(model_output, state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, |
||||
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)) |
||||
pm.send("cameraOdometry", create_pose_msg(model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)) |
||||
|
||||
# print("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f" % |
||||
# ((mt2 - mt1)*1000, (mt1 - last)*1000, meta_extra.frame_id, frame_id, frame_drop_ratio)) |
||||
# last = mt1 |
||||
last_vipc_frame_id = meta_main.frame_id |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
try: |
||||
main() |
||||
except KeyboardInterrupt: |
||||
sys.exit() |
||||
@ -0,0 +1,29 @@ |
||||
# distutils: language = c++ |
||||
|
||||
from libcpp cimport bool |
||||
from libc.stdint cimport uint32_t, uint64_t |
||||
|
||||
from .commonmodel cimport mat3 |
||||
|
||||
cdef extern from "cereal/messaging/messaging.h": |
||||
cdef cppclass MessageBuilder: |
||||
size_t getSerializedSize() |
||||
int serializeToBuffer(unsigned char *, size_t) |
||||
|
||||
cdef extern from "selfdrive/modeld/models/driving.h": |
||||
cdef int FEATURE_LEN |
||||
cdef int HISTORY_BUFFER_LEN |
||||
cdef int DESIRE_LEN |
||||
cdef int TRAFFIC_CONVENTION_LEN |
||||
cdef int DRIVING_STYLE_LEN |
||||
cdef int NAV_FEATURE_LEN |
||||
cdef int NAV_INSTRUCTION_LEN |
||||
cdef int OUTPUT_SIZE |
||||
cdef int NET_OUTPUT_SIZE |
||||
cdef int MODEL_FREQ |
||||
cdef bool CPP_USE_THNEED |
||||
cdef struct PublishState: pass |
||||
|
||||
mat3 update_calibration(float *, bool, bool) |
||||
void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool) |
||||
void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool) |
||||
@ -0,0 +1,60 @@ |
||||
# distutils: language = c++ |
||||
# cython: c_string_encoding=ascii |
||||
|
||||
import numpy as np |
||||
cimport numpy as cnp |
||||
from libcpp cimport bool |
||||
from libc.string cimport memcpy |
||||
from libc.stdint cimport uint32_t, uint64_t |
||||
|
||||
from .commonmodel cimport mat3 |
||||
from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \ |
||||
TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \ |
||||
NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \ |
||||
OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ, CPP_USE_THNEED |
||||
from .driving cimport MessageBuilder, PublishState as cppPublishState |
||||
from .driving cimport fill_model_msg, fill_pose_msg, update_calibration as cpp_update_calibration |
||||
|
||||
FEATURE_LEN = CPP_FEATURE_LEN |
||||
HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN |
||||
DESIRE_LEN = CPP_DESIRE_LEN |
||||
TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN |
||||
DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN |
||||
NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN |
||||
NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN |
||||
OUTPUT_SIZE = CPP_OUTPUT_SIZE |
||||
NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE |
||||
MODEL_FREQ = CPP_MODEL_FREQ |
||||
USE_THNEED = CPP_USE_THNEED |
||||
|
||||
cdef class PublishState: |
||||
cdef cppPublishState state |
||||
|
||||
def update_calibration(float[:] device_from_calib_euler, bool wide_camera, bool bigmodel_frame): |
||||
cdef mat3 result = cpp_update_calibration(&device_from_calib_euler[0], wide_camera, bigmodel_frame) |
||||
np_result = np.empty(9, dtype=np.float32) |
||||
cdef float[:] np_result_view = np_result |
||||
memcpy(&np_result_view[0], &result.v[0], 9*sizeof(float)) |
||||
return np_result.reshape(3, 3) |
||||
|
||||
def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, |
||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid): |
||||
cdef MessageBuilder msg |
||||
fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop, |
||||
timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid) |
||||
|
||||
output_size = msg.getSerializedSize() |
||||
output_data = bytearray(output_size) |
||||
cdef unsigned char * output_ptr = output_data |
||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" |
||||
return bytes(output_data) |
||||
|
||||
def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid): |
||||
cdef MessageBuilder msg |
||||
fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid) |
||||
|
||||
output_size = msg.getSerializedSize() |
||||
output_data = bytearray(output_size) |
||||
cdef unsigned char * output_ptr = output_data |
||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" |
||||
return bytes(output_data) |
||||
Loading…
Reference in new issue