sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Run models in function run_model (#20272)

Browse Source 
* reconnect vipc_client after disconnection

* run models in run_model()

* cleanup

* use std::Mutex

* use std::thread

* const

* comments

* cleanup include

* rename live_thread to calibration_thread

* add comment
pull/19857/head
Dean Lee 4 years ago committed by GitHub
parent cfe0338284
commit a7672e2851
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 109 additions and 134 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 65
      selfdrive/modeld/dmonitoringmodeld.cc
  2. 178
      selfdrive/modeld/modeld.cc

65
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -1,10 +1,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <cassert>
#include <sys/resource.h>
#include "visionbuf.h"
#include "visionipc_client.h"
#include "common/swaglog.h"
#include "common/util.h"
@ -15,51 +12,47 @@
#include <linux/limits.h>
#endif
ExitHandler do_exit;
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
PubMaster pm({"driverState"});
double last = 0;
// init the models
DMonitoringModelState dmonitoringmodel;
dmonitoring_init(&dmonitoringmodel);
while (!do_exit) {
VisionIpcBufExtra extra = {};
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr) continue;
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
while (!do_exit){
if (!vipc_client.connect(false)){
util::sleep_for(100);
continue;
}
break;
}
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
double t2 = millis_since_boot();
while (!do_exit) {
LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
double last = 0;
while (!do_exit) {
VisionIpcBufExtra extra = {0};
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr){
continue;
}
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2 - t1, t1 - last);
last = t1;
}
}
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height);
double t2 = millis_since_boot();
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output);
// init the models
DMonitoringModelState model;
dmonitoring_init(&model);
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;
}
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
dmonitoring_free(&dmonitoringmodel);
// run the models
if (vipc_client.connected) {
LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
run_model(model, vipc_client);
}
dmonitoring_free(&model);
return 0;
}

178
selfdrive/modeld/modeld.cc
Unescape View File

@ -1,9 +1,8 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <mutex>
#include <eigen3/Eigen/Dense>
#include "visionbuf.h"
#include "visionipc_client.h"
#include "common/swaglog.h"
#include "common/clutil.h"
@ -14,12 +13,12 @@
ExitHandler do_exit;
// globals
bool run_model;
bool live_calib_seen;
mat3 cur_transform;
pthread_mutex_t transform_lock;
std::mutex transform_lock;
void* live_thread(void *arg) {
set_thread_name("live");
void calibration_thread() {
set_thread_name("calibration");
set_realtime_priority(50);
SubMaster sm({"liveCalibration"});
@ -60,17 +59,77 @@ void* live_thread(void *arg) {
transform.v[i] = warp_matrix(i / 3, i % 3);
}
mat3 model_transform = matmul3(yuv_transform, transform);
pthread_mutex_lock(&transform_lock);
std::lock_guard lk(transform_lock);
cur_transform = model_transform;
run_model = true;
pthread_mutex_unlock(&transform_lock);
live_calib_seen = true;
}
}
}
void run_model(ModelState &model, VisionIpcClient &vipc_client) {
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "roadCameraState"});
// setup filter to track dropped frames
const float dt = 1. / MODEL_FREQ;
const float ts = 10.0; // filter time constant (s)
const float frame_filter_k = (dt / ts) / (1. + dt / ts);
float frames_dropped = 0;
uint32_t frame_id = 0, last_vipc_frame_id = 0;
double last = 0;
int desire = -1;
uint32_t run_count = 0;
while (!do_exit) {
VisionIpcBufExtra extra = {};
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr) continue;
transform_lock.lock();
mat3 model_transform = cur_transform;
const bool run_model_this_iter = live_calib_seen;
transform_lock.unlock();
if (sm.update(0) > 0) {
// TODO: path planner timeout?
desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
}
if (run_model_this_iter) {
run_count++;
float vec_desire[DESIRE_LEN] = {0};
if (desire >= 0 && desire < DESIRE_LEN) {
vec_desire[desire] = 1.0;
}
double mt1 = millis_since_boot();
ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
model_transform, vec_desire);
double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
// tracked dropped frames
uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
if (run_count < 10) frames_dropped = 0; // let frame drops warm up
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
kj::ArrayPtr<const float>(model.output.data(), model.output.size()));
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
last_vipc_frame_id = extra.frame_id;
}
}
return NULL;
}
int main(int argc, char **argv) {
int err;
set_realtime_priority(54);
#ifdef QCOM
@ -80,16 +139,8 @@ int main(int argc, char **argv) {
set_core_affinity(4);
#endif
pthread_mutex_init(&transform_lock, NULL);
// start calibration thread
pthread_t live_thread_handle;
err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
assert(err == 0);
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "roadCameraState"});
std::thread thread = std::thread(calibration_thread);
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
@ -101,90 +152,21 @@ int main(int argc, char **argv) {
LOGW("models loaded, modeld starting");
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context);
while (!do_exit){
if (!vipc_client.connect(false)){
util::sleep_for(100);
continue;
}
break;
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
// loop
while (!do_exit) {
VisionBuf *b = &vipc_client.buffers[0];
// run the models
// vipc_client.connected is false only when do_exit is true
if (vipc_client.connected) {
const VisionBuf *b = &vipc_client.buffers[0];
LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height);
// setup filter to track dropped frames
const float dt = 1. / MODEL_FREQ;
const float ts = 10.0; // filter time constant (s)
const float frame_filter_k = (dt / ts) / (1. + dt / ts);
float frames_dropped = 0;
uint32_t frame_id = 0, last_vipc_frame_id = 0;
double last = 0;
int desire = -1;
uint32_t run_count = 0;
while (!do_exit) {
VisionIpcBufExtra extra;
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr){
continue;
}
pthread_mutex_lock(&transform_lock);
mat3 model_transform = cur_transform;
const bool run_model_this_iter = run_model;
pthread_mutex_unlock(&transform_lock);
if (sm.update(0) > 0){
// TODO: path planner timeout?
desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
}
double mt1 = 0, mt2 = 0;
if (run_model_this_iter) {
run_count++;
float vec_desire[DESIRE_LEN] = {0};
if (desire >= 0 && desire < DESIRE_LEN) {
vec_desire[desire] = 1.0;
}
mt1 = millis_since_boot();
ModelDataRaw model_buf =
model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
model_transform, vec_desire);
mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
// tracked dropped frames
uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
if (run_count < 10) frames_dropped = 0; // let frame drops warm up
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
kj::ArrayPtr<const float>(model.output.data(), model.output.size()));
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
last_vipc_frame_id = extra.frame_id;
}
}
run_model(model, vipc_client);
}
model_free(&model);
LOG("joining live_thread");
err = pthread_join(live_thread_handle, NULL);
assert(err == 0);
LOG("joining calibration thread");
thread.join();
CL_CHECK(clReleaseContext(context));
pthread_mutex_destroy(&transform_lock);
return 0;
}

Write Preview
Loading…
Cancel
Save