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

178
selfdrive/modeld/modeld.cc
Unescape View File

@ -1,9 +1,8 @@
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h> #include <mutex>
#include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Dense>
#include "visionbuf.h"
#include "visionipc_client.h" #include "visionipc_client.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/clutil.h" #include "common/clutil.h"
@ -14,12 +13,12 @@
ExitHandler do_exit; ExitHandler do_exit;
// globals // globals
bool run_model; bool live_calib_seen;
mat3 cur_transform; mat3 cur_transform;
pthread_mutex_t transform_lock; std::mutex transform_lock;
void* live_thread(void *arg) { void calibration_thread() {
set_thread_name("live"); set_thread_name("calibration");
set_realtime_priority(50); set_realtime_priority(50);
SubMaster sm({"liveCalibration"}); SubMaster sm({"liveCalibration"});
@ -60,17 +59,77 @@ void* live_thread(void *arg) {
transform.v[i] = warp_matrix(i / 3, i % 3); transform.v[i] = warp_matrix(i / 3, i % 3);
} }
mat3 model_transform = matmul3(yuv_transform, transform); mat3 model_transform = matmul3(yuv_transform, transform);
pthread_mutex_lock(&transform_lock); std::lock_guard lk(transform_lock);
cur_transform = model_transform; cur_transform = model_transform;
run_model = true; live_calib_seen = true;
pthread_mutex_unlock(&transform_lock); }
}
}
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 main(int argc, char **argv) {
int err;
set_realtime_priority(54); set_realtime_priority(54);
#ifdef QCOM #ifdef QCOM
@ -80,16 +139,8 @@ int main(int argc, char **argv) {
set_core_affinity(4); set_core_affinity(4);
#endif #endif
pthread_mutex_init(&transform_lock, NULL);
// start calibration thread // start calibration thread
pthread_t live_thread_handle; std::thread thread = std::thread(calibration_thread);
err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
assert(err == 0);
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "roadCameraState"});
// cl init // cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); 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"); LOGW("models loaded, modeld starting");
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context);
while (!do_exit && !vipc_client.connect(false)) {
while (!do_exit){ util::sleep_for(100);
if (!vipc_client.connect(false)){
util::sleep_for(100);
continue;
}
break;
} }
// loop // run the models
while (!do_exit) { // vipc_client.connected is false only when do_exit is true
VisionBuf *b = &vipc_client.buffers[0]; 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); LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height);
run_model(model, vipc_client);
// 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;
}
}
} }
model_free(&model); model_free(&model);
LOG("joining calibration thread");
LOG("joining live_thread"); thread.join();
err = pthread_join(live_thread_handle, NULL);
assert(err == 0);
CL_CHECK(clReleaseContext(context)); CL_CHECK(clReleaseContext(context));
pthread_mutex_destroy(&transform_lock);
return 0; return 0;
} }

Write Preview
Loading…
Cancel
Save