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. 55
      selfdrive/modeld/dmonitoringmodeld.cc
  2. 122
      selfdrive/modeld/modeld.cc

55
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"});
// init the models
DMonitoringModelState dmonitoringmodel;
dmonitoring_init(&dmonitoringmodel);
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
while (!do_exit){
if (!vipc_client.connect(false)){
util::sleep_for(100);
continue;
}
break;
}
while (!do_exit) {
LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
double last = 0; double last = 0;
while (!do_exit) { while (!do_exit) {
VisionIpcBufExtra extra = {0}; VisionIpcBufExtra extra = {};
VisionBuf *buf = vipc_client.recv(&extra); VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr){ if (buf == nullptr) continue;
continue;
}
double t1 = millis_since_boot(); double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height); DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
double t2 = millis_since_boot(); double t2 = millis_since_boot();
// send dm packet // send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output); dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2 - t1, t1 - last); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2 - t1, t1 - last);
last = t1; last = t1;
} }
} }
dmonitoring_free(&dmonitoringmodel); int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
// init the models
DMonitoringModelState model;
dmonitoring_init(&model);
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
// 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;
} }

122
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,61 +59,18 @@ 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);
} }
} }
return NULL;
} }
int main(int argc, char **argv) { void run_model(ModelState &model, VisionIpcClient &vipc_client) {
int err;
set_realtime_priority(54);
#ifdef QCOM
set_core_affinity(2);
#elif QCOM2
// CPU usage is much lower when pinned to a single big core
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 // messaging
PubMaster pm({"modelV2", "cameraOdometry"}); PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "roadCameraState"}); SubMaster sm({"lateralPlan", "roadCameraState"});
// 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");
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;
}
// loop
while (!do_exit) {
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 // setup filter to track dropped frames
const float dt = 1. / MODEL_FREQ; const float dt = 1. / MODEL_FREQ;
const float ts = 10.0; // filter time constant (s) const float ts = 10.0; // filter time constant (s)
@ -127,16 +83,14 @@ int main(int argc, char **argv) {
uint32_t run_count = 0; uint32_t run_count = 0;
while (!do_exit) { while (!do_exit) {
VisionIpcBufExtra extra; VisionIpcBufExtra extra = {};
VisionBuf *buf = vipc_client.recv(&extra); VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr){ if (buf == nullptr) continue;
continue;
}
pthread_mutex_lock(&transform_lock); transform_lock.lock();
mat3 model_transform = cur_transform; mat3 model_transform = cur_transform;
const bool run_model_this_iter = run_model; const bool run_model_this_iter = live_calib_seen;
pthread_mutex_unlock(&transform_lock); transform_lock.unlock();
if (sm.update(0) > 0) { if (sm.update(0) > 0) {
// TODO: path planner timeout? // TODO: path planner timeout?
@ -144,7 +98,6 @@ int main(int argc, char **argv) {
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
} }
double mt1 = 0, mt2 = 0;
if (run_model_this_iter) { if (run_model_this_iter) {
run_count++; run_count++;
@ -153,12 +106,10 @@ int main(int argc, char **argv) {
vec_desire[desire] = 1.0; vec_desire[desire] = 1.0;
} }
mt1 = millis_since_boot(); double mt1 = millis_since_boot();
ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
ModelDataRaw model_buf =
model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
model_transform, vec_desire); model_transform, vec_desire);
mt2 = millis_since_boot(); double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0; float model_execution_time = (mt2 - mt1) / 1000.0;
// tracked dropped frames // tracked dropped frames
@ -175,16 +126,47 @@ int main(int argc, char **argv) {
last = mt1; last = mt1;
last_vipc_frame_id = extra.frame_id; last_vipc_frame_id = extra.frame_id;
} }
}
}
int main(int argc, char **argv) {
set_realtime_priority(54);
#ifdef QCOM
set_core_affinity(2);
#elif QCOM2
// CPU usage is much lower when pinned to a single big core
set_core_affinity(4);
#endif
// start calibration thread
std::thread thread = std::thread(calibration_thread);
// 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");
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
} }
// 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);
run_model(model, vipc_client);
} }
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