mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-26 07:32:06 +08:00
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
old-commit-hash: a7672e2851
This commit is contained in:
Dean Lee
2021-03-09 17:32:13 +08:00
committed by GitHub
parent db579da318
commit ee6d78c90b
2 changed files with 109 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/dmonitoringmodeld.cc
+29 -36
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;
void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
PubMaster pm({"driverState"});
double last = 0;
while (!do_exit) {
VisionIpcBufExtra extra = {};
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr) continue;
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
double t2 = millis_since_boot();
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2 - t1, t1 - last);
last = t1;
}
}
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
PubMaster pm({"driverState"});
// init the models
DMonitoringModelState dmonitoringmodel;
dmonitoring_init(&dmonitoringmodel);
DMonitoringModelState model;
dmonitoring_init(&model);
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 && !vipc_client.connect(false)) {
util::sleep_for(100);
}
while (!do_exit) {
// run the models
if (vipc_client.connected) {
LOGW("connected with buffer size: %d", vipc_client.buffers[0].len);
double last = 0;
while (!do_exit) {
VisionIpcBufExtra extra = {0};
VisionBuf *buf = vipc_client.recv(&extra);
if (buf == nullptr){
continue;
}
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height);
double t2 = millis_since_boot();
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output);
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;
}
run_model(model, vipc_client);
}
dmonitoring_free(&dmonitoringmodel);
dmonitoring_free(&model);
return 0;
}
selfdrive/modeld/modeld.cc
+80 -98
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;
}