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

fullframe DM model (#24860)

Browse Source 
* Revert "put cereal on master"

This reverts commit a8ccd8f838.

* Revert "Revert fullframe DM model (#24812)"

This reverts commit c646eeee0a.

* revert revert cereal

* clip6

* 0.8 is fair

* Fiction compensation should be based on error

* Update refs

* Add deadzone

* not that

* good mg

* ref

* ref

* ee8f

* minor tweak

* ref

* recompile

* ref

* cereal

* match driverstatus

* new ref

* new ref

* pass token through jenkins credentials

* quote

* fix snpe dead weights

* final ref

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
pull/24918/head
ZwX1616 3 years ago committed by GitHub
parent 8270c1c1a3
commit 1f2f9ea9c9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
24 changed files with 229 additions and 308 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. 2
      Jenkinsfile
  2. 2
      cereal
  3. 6
      common/modeldata.h
  4. 6
      selfdrive/modeld/dmonitoringmodeld.cc
  5. 224
      selfdrive/modeld/models/dmonitoring.cc
  6. 33
      selfdrive/modeld/models/dmonitoring.h
  7. 4
      selfdrive/modeld/models/dmonitoring_model.current
  8. 4
      selfdrive/modeld/models/dmonitoring_model.onnx
  9. 4
      selfdrive/modeld/models/dmonitoring_model_q.dlc
  10. 21
      selfdrive/modeld/runners/onnx_runner.py
  11. 6
      selfdrive/modeld/runners/onnxmodel.cc
  12. 3
      selfdrive/modeld/runners/onnxmodel.h
  13. 12
      selfdrive/modeld/runners/snpemodel.cc
  14. 3
      selfdrive/modeld/runners/snpemodel.h
  15. 7
      selfdrive/monitoring/dmonitoringd.py
  16. 94
      selfdrive/monitoring/driver_monitor.py
  17. 24
      selfdrive/monitoring/test_monitoring.py
  18. 8
      selfdrive/test/process_replay/model_replay.py
  19. 2
      selfdrive/test/process_replay/model_replay_ref_commit
  20. 2
      selfdrive/test/process_replay/process_replay.py
  21. 6
      selfdrive/test/test_onroad.py
  22. 43
      selfdrive/ui/qt/offroad/driverview.cc
  23. 9
      selfdrive/ui/qt/widgets/cameraview.cc
  24. 2
      system/hardware/tici/test_power_draw.py

2
Jenkinsfile vendored
Unescape View File

@ -10,6 +10,7 @@ export TEST_DIR=${env.TEST_DIR}
export SOURCE_DIR=${env.SOURCE_DIR}
export GIT_BRANCH=${env.GIT_BRANCH}
export GIT_COMMIT=${env.GIT_COMMIT}
export AZURE_TOKEN='${env.AZURE_TOKEN}'
source ~/.bash_profile
if [ -f /TICI ]; then
@ -45,6 +46,7 @@ pipeline {
CI = "1"
TEST_DIR = "/data/openpilot"
SOURCE_DIR = "/data/openpilot_source/"
AZURE_TOKEN = credentials('azure_token')
}
options {
timeout(time: 4, unit: 'HOURS')

2
cereal

@ -1 +1 @@
Subproject commit f445245a8fbe01c33a372d82897575e7acc08ffc
Subproject commit 05bbdafb67060ffb62e6e0af812935494ad91933

6
common/modeldata.h
Unescape View File

@ -24,12 +24,6 @@ constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
namespace tici_dm_crop {
const int x_offset = -72;
const int y_offset = -144;
const int width = 954;
};
const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2,
0.0, 2648.0, 1208.0 / 2,
0.0, 0.0, 1.0}};

6
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -12,7 +12,7 @@
ExitHandler do_exit;
void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
PubMaster pm({"driverState"});
PubMaster pm({"driverStateV2"});
SubMaster sm({"liveCalibration"});
float calib[CALIB_LEN] = {0};
double last = 0;
@ -31,11 +31,11 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
}
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib);
DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, buf->stride, buf->uv_offset, calib);
double t2 = millis_since_boot();
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output);
//printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last);
last = t1;

224
selfdrive/modeld/models/dmonitoring.cc
Unescape View File

@ -10,8 +10,8 @@
#include "selfdrive/modeld/models/dmonitoring.h"
constexpr int MODEL_WIDTH = 320;
constexpr int MODEL_HEIGHT = 640;
constexpr int MODEL_WIDTH = 1440;
constexpr int MODEL_HEIGHT = 960;
template <class T>
static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
@ -19,199 +19,115 @@ static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
return buf.data();
}
static inline void init_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
uint8_t *y = get_buffer(buf, width * height * 3 / 2);
uint8_t *u = y + width * height;
uint8_t *v = u + (width / 2) * (height / 2);
// needed on comma two to make the padded border black
// equivalent to RGB(0,0,0) in YUV space
memset(y, 16, width * height);
memset(u, 128, (width / 2) * (height / 2));
memset(v, 128, (width / 2) * (height / 2));
}
void dmonitoring_init(DMonitoringModelState* s) {
s->is_rhd = Params().getBool("IsRHD");
for (int x = 0; x < std::size(s->tensor); ++x) {
s->tensor[x] = (x - 128.f) * 0.0078125f;
}
init_yuv_buf(s->resized_buf, MODEL_WIDTH, MODEL_HEIGHT);
#ifdef USE_ONNX_MODEL
s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME);
s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#else
s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME);
s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#endif
s->m->addCalib(s->calib, CALIB_LEN);
}
static inline auto get_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
uint8_t *y = get_buffer(buf, width * height * 3 / 2);
uint8_t *u = y + width * height;
uint8_t *v = u + (width /2) * (height / 2);
return std::make_tuple(y, u, v);
void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) {
for (int i = 0; i < 3; ++i) {
ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE;
ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]);
}
struct Rect {int x, y, w, h;};
void crop_nv12_to_yuv(uint8_t *raw, int stride, int uv_offset, uint8_t *y, uint8_t *u, uint8_t *v, const Rect &rect) {
uint8_t *raw_y = raw;
uint8_t *raw_uv = raw_y + uv_offset;
for (int r = 0; r < rect.h / 2; r++) {
memcpy(y + 2 * r * rect.w, raw_y + (2 * r + rect.y) * stride + rect.x, rect.w);
memcpy(y + (2 * r + 1) * rect.w, raw_y + (2 * r + rect.y + 1) * stride + rect.x, rect.w);
for (int h = 0; h < rect.w / 2; h++) {
u[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2];
v[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2 + 1];
for (int i = 0; i < 2; ++i) {
ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE;
ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]);
}
for (int i = 0; i < 4; ++i) {
ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]);
}
for (int i = 0; i < 2; ++i) {
ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]);
}
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
const int cropped_height = tici_dm_crop::width / 1.33;
Rect crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset,
height / 2 - cropped_height / 2 + tici_dm_crop::y_offset,
cropped_height / 2,
cropped_height};
if (!s->is_rhd) {
crop_rect.x += tici_dm_crop::width - crop_rect.w;
ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]);
ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]);
ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]);
ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]);
ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]);
ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]);
ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]);
}
int resized_width = MODEL_WIDTH;
int resized_height = MODEL_HEIGHT;
auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h);
if (!s->is_rhd) {
crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, cropped_y, cropped_u, cropped_v, crop_rect);
} else {
auto [mirror_y, mirror_u, mirror_v] = get_yuv_buf(s->premirror_cropped_buf, crop_rect.w, crop_rect.h);
crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, mirror_y, mirror_u, mirror_v, crop_rect);
libyuv::I420Mirror(mirror_y, crop_rect.w,
mirror_u, crop_rect.w / 2,
mirror_v, crop_rect.w / 2,
cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h);
void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) {
ddata.setFaceOrientation(ds_res.face_orientation);
ddata.setFaceOrientationStd(ds_res.face_orientation_std);
ddata.setFacePosition(ds_res.face_position);
ddata.setFacePositionStd(ds_res.face_position_std);
ddata.setFaceProb(ds_res.face_prob);
ddata.setLeftEyeProb(ds_res.left_eye_prob);
ddata.setRightEyeProb(ds_res.right_eye_prob);
ddata.setLeftBlinkProb(ds_res.left_blink_prob);
ddata.setRightBlinkProb(ds_res.right_blink_prob);
ddata.setSunglassesProb(ds_res.sunglasses_prob);
ddata.setOccludedProb(ds_res.occluded_prob);
ddata.setReadyProb(ds_res.ready_prob);
ddata.setNotReadyProb(ds_res.not_ready_prob);
}
auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height);
uint8_t *resized_y = resized_buf;
libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear;
libyuv::I420Scale(cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h,
resized_y, resized_width,
resized_u, resized_width / 2,
resized_v, resized_width / 2,
resized_width, resized_height,
mode);
int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
// one shot conversion, O(n) anyway
// yuvframe2tensor, normalize
for (int r = 0; r < MODEL_HEIGHT/2; r++) {
for (int c = 0; c < MODEL_WIDTH/2; c++) {
// Y_ul
net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c]];
// Y_dl
net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c]];
// Y_ur
net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c+1]];
// Y_dr
net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c+1]];
// U
net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_u[r*resized_width/2 + c]];
// V
net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_v[r*resized_width/2 + c]];
}
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
int v_off = height - MODEL_HEIGHT;
int h_off = (width - MODEL_WIDTH) / 2;
int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT;
uint8_t *raw_buf = (uint8_t *) stream_buf;
// vertical crop free
uint8_t *raw_y_start = raw_buf + stride * v_off;
uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
// here makes a uint8 copy
for (int r = 0; r < MODEL_HEIGHT; ++r) {
memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH);
}
// printf("preprocess completed. %d \n", yuv_buf_len);
// FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb");
//fwrite(resized_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
// fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
// fclose(dump_yuv_file);
// *** testing ***
// idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320)
// imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420))
//FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb");
//fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
//fclose(dump_yuv_file2);
double t1 = millis_since_boot();
s->m->addImage(net_input_buf, yuv_buf_len);
s->m->addImage((float*)net_input_buf, yuv_buf_len / 4);
for (int i = 0; i < CALIB_LEN; i++) {
s->calib[i] = calib[i];
}
s->m->execute();
double t2 = millis_since_boot();
DMonitoringResult ret = {0};
for (int i = 0; i < 3; ++i) {
ret.face_orientation[i] = s->output[i] * REG_SCALE;
ret.face_orientation_meta[i] = exp(s->output[6 + i]);
}
for (int i = 0; i < 2; ++i) {
ret.face_position[i] = s->output[3 + i] * REG_SCALE;
ret.face_position_meta[i] = exp(s->output[9 + i]);
}
for (int i = 0; i < 4; ++i) {
ret.ready_prob[i] = sigmoid(s->output[39 + i]);
}
for (int i = 0; i < 2; ++i) {
ret.not_ready_prob[i] = sigmoid(s->output[43 + i]);
}
ret.face_prob = sigmoid(s->output[12]);
ret.left_eye_prob = sigmoid(s->output[21]);
ret.right_eye_prob = sigmoid(s->output[30]);
ret.left_blink_prob = sigmoid(s->output[31]);
ret.right_blink_prob = sigmoid(s->output[32]);
ret.sg_prob = sigmoid(s->output[33]);
ret.poor_vision = sigmoid(s->output[34]);
ret.partial_face = sigmoid(s->output[35]);
ret.distracted_pose = sigmoid(s->output[36]);
ret.distracted_eyes = sigmoid(s->output[37]);
ret.occluded_prob = sigmoid(s->output[38]);
ret.dsp_execution_time = (t2 - t1) / 1000.;
return ret;
DMonitoringModelResult model_res = {0};
parse_driver_data(model_res.driver_state_lhd, s, 0);
parse_driver_data(model_res.driver_state_rhd, s, 41);
model_res.poor_vision_prob = sigmoid(s->output[82]);
model_res.wheel_on_right_prob = sigmoid(s->output[83]);
model_res.dsp_execution_time = (t2 - t1) / 1000.;
return model_res;
}
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent().initDriverState();
auto framed = msg.initEvent().initDriverStateV2();
framed.setFrameId(frame_id);
framed.setModelExecutionTime(execution_time);
framed.setDspExecutionTime(res.dsp_execution_time);
framed.setFaceOrientation(res.face_orientation);
framed.setFaceOrientationStd(res.face_orientation_meta);
framed.setFacePosition(res.face_position);
framed.setFacePositionStd(res.face_position_meta);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
framed.setSunglassesProb(res.sg_prob);
framed.setPoorVision(res.poor_vision);
framed.setPartialFace(res.partial_face);
framed.setDistractedPose(res.distracted_pose);
framed.setDistractedEyes(res.distracted_eyes);
framed.setOccludedProb(res.occluded_prob);
framed.setReadyProb(res.ready_prob);
framed.setNotReadyProb(res.not_ready_prob);
framed.setDspExecutionTime(model_res.dsp_execution_time);
framed.setPoorVisionProb(model_res.poor_vision_prob);
framed.setWheelOnRightProb(model_res.wheel_on_right_prob);
fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd);
fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd);
if (send_raw_pred) {
framed.setRawPredictions(raw_pred.asBytes());
}
pm.send("driverState", msg);
pm.send("driverStateV2", msg);
}
void dmonitoring_free(DMonitoringModelState* s) {

33
selfdrive/modeld/models/dmonitoring.h
Unescape View File

@ -9,44 +9,43 @@
#define CALIB_LEN 3
#define OUTPUT_SIZE 45
#define OUTPUT_SIZE 84
#define REG_SCALE 0.25f
typedef struct DMonitoringResult {
typedef struct DriverStateResult {
float face_orientation[3];
float face_orientation_meta[3];
float face_orientation_std[3];
float face_position[2];
float face_position_meta[2];
float face_position_std[2];
float face_prob;
float left_eye_prob;
float right_eye_prob;
float left_blink_prob;
float right_blink_prob;
float sg_prob;
float poor_vision;
float partial_face;
float distracted_pose;
float distracted_eyes;
float sunglasses_prob;
float occluded_prob;
float ready_prob[4];
float not_ready_prob[2];
} DriverStateResult;
typedef struct DMonitoringModelResult {
DriverStateResult driver_state_lhd;
DriverStateResult driver_state_rhd;
float poor_vision_prob;
float wheel_on_right_prob;
float dsp_execution_time;
} DMonitoringResult;
} DMonitoringModelResult;
typedef struct DMonitoringModelState {
RunModel *m;
bool is_rhd;
float output[OUTPUT_SIZE];
std::vector<uint8_t> resized_buf;
std::vector<uint8_t> cropped_buf;
std::vector<uint8_t> premirror_cropped_buf;
std::vector<float> net_input_buf;
std::vector<uint8_t> net_input_buf;
float calib[CALIB_LEN];
float tensor[UINT8_MAX + 1];
} DMonitoringModelState;
void dmonitoring_init(DMonitoringModelState* s);
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred);
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
void dmonitoring_free(DMonitoringModelState* s);

4
selfdrive/modeld/models/dmonitoring_model.current
Unescape View File

@ -1,2 +1,2 @@
a8236e30-5bee-4689-8ea0-fc102e2770e5
d508c79bae1c1c451f3af3e2bc231ce33678cb43
ee8f830b-d6a1-42ef-9b1b-50fd0b2faae4
cac8f7b69d420506707ff7a19d573d5011ef2533

4
selfdrive/modeld/models/dmonitoring_model.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:00731ebd06fcff7e5837607b91bc56cad3bed5d7ee89052c911c981e8f665308
size 3679940
oid sha256:932e589e5cce66e5d9f48492426a33c74cd7f352a870d3ddafcede3e9156f30d
size 9157561

4
selfdrive/modeld/models/dmonitoring_model_q.dlc
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:667df5e925570a0f6a33dfb890e186a1f13f101885b46db47ec45305737dffb6
size 1145921
oid sha256:3587976a8b7e3be274fa86c2e2233e3e464cad713f5077c4394cd1ddd3c7c6c5
size 2636965

21
selfdrive/modeld/runners/onnx_runner.py
Unescape View File

@ -9,20 +9,24 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
import onnxruntime as ort # pylint: disable=import-error
def read(sz):
def read(sz, tf8=False):
dd = []
gt = 0
while gt < sz * 4:
st = os.read(0, sz * 4 - gt)
szof = 1 if tf8 else 4
while gt < sz * szof:
st = os.read(0, sz * szof - gt)
assert(len(st) > 0)
dd.append(st)
gt += len(st)
return np.frombuffer(b''.join(dd), dtype=np.float32)
r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32)
if tf8:
r = r / 255.
return r
def write(d):
os.write(1, d.tobytes())
def run_loop(m):
def run_loop(m, tf8_input=False):
ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()]
keys = [x.name for x in m.get_inputs()]
@ -33,10 +37,10 @@ def run_loop(m):
print("ready to run onnx model", keys, ishapes, file=sys.stderr)
while 1:
inputs = []
for shp in ishapes:
for k, shp in zip(keys, ishapes):
ts = np.product(shp)
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
inputs.append(read(ts).reshape(shp))
inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp))
ret = m.run(None, dict(zip(keys, inputs)))
#print(ret, file=sys.stderr)
for r in ret:
@ -44,6 +48,7 @@ def run_loop(m):
if __name__ == "__main__":
print(sys.argv, file=sys.stderr)
print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr)
options = ort.SessionOptions()
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
@ -63,6 +68,6 @@ if __name__ == "__main__":
print("Onnx selected provider: ", [provider], file=sys.stderr)
ort_session = ort.InferenceSession(sys.argv[1], options, providers=[provider])
print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
run_loop(ort_session)
run_loop(ort_session, tf8_input=("--use_tf8" in sys.argv))
except KeyboardInterrupt:
pass

6
selfdrive/modeld/runners/onnxmodel.cc
Unescape View File

@ -14,12 +14,13 @@
#include "common/swaglog.h"
#include "common/util.h"
ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra) {
ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime, bool _use_extra, bool _use_tf8) {
LOGD("loading model %s", path);
output = _output;
output_size = _output_size;
use_extra = _use_extra;
use_tf8 = _use_tf8;
int err = pipe(pipein);
assert(err == 0);
@ -28,11 +29,12 @@ ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int
std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe"));
std::string onnx_runner = exe_dir + "/runners/onnx_runner.py";
std::string tf8_arg = use_tf8 ? "--use_tf8" : "";
proc_pid = fork();
if (proc_pid == 0) {
LOGD("spawning onnx process %s", onnx_runner.c_str());
char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, nullptr};
char *argv[] = {(char*)onnx_runner.c_str(), (char*)path, (char*)tf8_arg.c_str(), nullptr};
dup2(pipein[0], 0);
dup2(pipeout[1], 1);
close(pipein[0]);

3
selfdrive/modeld/runners/onnxmodel.h
Unescape View File

@ -6,7 +6,7 @@
class ONNXModel : public RunModel {
public:
ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false);
ONNXModel(const char *path, float *output, size_t output_size, int runtime, bool use_extra = false, bool _use_tf8 = false);
~ONNXModel();
void addRecurrent(float *state, int state_size);
void addDesire(float *state, int state_size);
@ -31,6 +31,7 @@ private:
int calib_size;
float *image_input_buf = NULL;
int image_buf_size;
bool use_tf8;
float *extra_input_buf = NULL;
int extra_buf_size;
bool use_extra;

12
selfdrive/modeld/runners/snpemodel.cc
Unescape View File

@ -14,10 +14,11 @@ void PrintErrorStringAndExit() {
std::exit(EXIT_FAILURE);
}
SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra) {
SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra, bool luse_tf8) {
output = loutput;
output_size = loutput_size;
use_extra = luse_extra;
use_tf8 = luse_tf8;
#ifdef QCOM2
if (runtime==USE_GPU_RUNTIME) {
Runtime = zdl::DlSystem::Runtime_t::GPU;
@ -70,14 +71,16 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int
printf("model: %s -> %s\n", input_tensor_name, output_tensor_name);
zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat;
zdl::DlSystem::UserBufferEncodingTf8 userBufferEncodingTf8(0, 1./255); // network takes 0-1
zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
size_t size_of_input = use_tf8 ? sizeof(uint8_t) : sizeof(float);
// create input buffer
{
const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name);
const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt;
std::vector<size_t> strides(bufferShape.rank());
strides[strides.size() - 1] = sizeof(float);
strides[strides.size() - 1] = size_of_input;
size_t product = 1;
for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i];
size_t stride = strides[strides.size() - 1];
@ -86,7 +89,10 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int
strides[i-1] = stride;
}
printf("input product is %lu\n", product);
inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat);
inputBuffer = ubFactory.createUserBuffer(NULL,
product*size_of_input,
strides,
use_tf8 ? (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingTf8 : (zdl::DlSystem::UserBufferEncoding*)&userBufferEncodingFloat);
inputMap.add(input_tensor_name, inputBuffer.get());
}

3
selfdrive/modeld/runners/snpemodel.h
Unescape View File

@ -23,7 +23,7 @@
class SNPEModel : public RunModel {
public:
SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false);
SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false, bool use_tf8 = false);
void addRecurrent(float *state, int state_size);
void addTrafficConvention(float *state, int state_size);
void addCalib(float *state, int state_size);
@ -52,6 +52,7 @@ private:
std::unique_ptr<zdl::DlSystem::IUserBuffer> inputBuffer;
float *input;
size_t input_size;
bool use_tf8;
// snpe output stuff
zdl::DlSystem::UserBufferMap outputMap;

7
selfdrive/monitoring/dmonitoringd.py
Unescape View File

@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None):
pm = messaging.PubMaster(['driverMonitoringState'])
if sm is None:
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState'])
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2'])
driver_status = DriverStatus(rhd=Params().get_bool("IsRHD"))
@ -34,7 +34,7 @@ def dmonitoringd_thread(sm=None, pm=None):
while True:
sm.update()
if not sm.updated['driverState']:
if not sm.updated['driverStateV2']:
continue
# Get interaction
@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None):
# Get data from dmonitoringmodeld
events = Events()
driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
@ -79,6 +79,7 @@ def dmonitoringd_thread(sm=None, pm=None):
"isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds,
"isActiveMode": driver_status.active_monitoring_mode,
"isRHD": driver_status.wheel_on_right,
}
pm.send('driverMonitoringState', dat)

94
selfdrive/monitoring/driver_monitor.py
Unescape View File

@ -5,6 +5,7 @@ from common.numpy_fast import interp
from common.realtime import DT_DMON
from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter
from common.transformations.camera import tici_d_frame_size
EventName = car.CarEvent.EventName
@ -25,33 +26,30 @@ class DRIVER_MONITOR_SETTINGS():
self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.5
self._PARTIAL_FACE_THRESHOLD = 0.8
self._FACE_THRESHOLD = 0.7
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.925
self._BLINK_THRESHOLD = 0.8
self._BLINK_THRESHOLD_SLACK = 0.9
self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
self._SG_THRESHOLD = 0.9
self._BLINK_THRESHOLD = 0.87
self._EE_THRESH11 = 0.75
self._EE_THRESH12 = 3.25
self._EE_THRESH21 = 0.01
self._EE_THRESH22 = 0.35
self._POSE_PITCH_THRESHOLD = 0.3237
self._POSE_PITCH_THRESHOLD_SLACK = 0.3657
self._POSE_PITCH_THRESHOLD = 0.3133
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
self._POSE_YAW_THRESHOLD = 0.3109
self._POSE_YAW_THRESHOLD_SLACK = 0.4294
self._POSE_YAW_THRESHOLD = 0.4020
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
self._PITCH_NATURAL_OFFSET = 0.057 # initial value before offset is learned
self._YAW_NATURAL_OFFSET = 0.11 # initial value before offset is learned
self._PITCH_NATURAL_OFFSET = 0.029 # initial value before offset is learned
self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned
self._PITCH_MAX_OFFSET = 0.124
self._PITCH_MIN_OFFSET = -0.0881
self._YAW_MAX_OFFSET = 0.289
self._YAW_MIN_OFFSET = -0.0246
self._POSESTD_THRESHOLD = 0.315
self._POSESTD_THRESHOLD = 0.3
self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
@ -59,6 +57,9 @@ class DRIVER_MONITOR_SETTINGS():
self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative
self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
self._WHEELPOS_THRESHOLD = 0.5
self._WHEELPOS_FILTER_MIN_COUNT = int(5 / self._DT_DMON)
self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change
self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
@ -66,9 +67,9 @@ class DRIVER_MONITOR_SETTINGS():
self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts
# model output refers to center of cropped image, so need to apply the x displacement offset
RESIZED_FOCAL = 320.0
H, W, FULL_W = 320, 160, 426
# model output refers to center of undistorted+leveled image
EFL = 598.0 # focal length in K
W, H = tici_d_frame_size # corrected image has same size as raw
class DistractedType:
NOT_DISTRACTED = 0
@ -76,22 +77,22 @@ class DistractedType:
DISTRACTED_BLINK = 2
DISTRACTED_E2E = 4
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
pitch_net, yaw_net, roll_net = angles_desc
face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
yaw_focal_angle = atan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL)
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, RESIZED_FOCAL)
face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
pitch = pitch_net + pitch_focal_angle
yaw = -yaw_net + yaw_focal_angle
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
yaw -= rpy_calib[2]
return roll_net, pitch, yaw
class DriverPose():
@ -112,7 +113,6 @@ class DriverBlink():
def __init__(self):
self.left_blink = 0.
self.right_blink = 0.
self.cfactor = 1.
class DriverStatus():
def __init__(self, rhd=False, settings=DRIVER_MONITOR_SETTINGS()):
@ -120,7 +120,7 @@ class DriverStatus():
self.settings = settings
# init driver status
self.is_rhd_region = rhd
# self.wheelpos_learner = RunningStatFilter()
self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT)
self.pose_calibrated = False
self.blink = DriverBlink()
@ -137,8 +137,8 @@ class DriverStatus():
self.distracted_types = []
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
self.wheel_on_right = rhd
self.face_detected = False
self.face_partial = False
self.terminal_alert_cnt = 0
self.terminal_time = 0
self.step_change = 0.
@ -197,7 +197,7 @@ class DriverStatus():
yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw:
distracted_types.append(DistractedType.DISTRACTED_POSE)
if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor:
if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD:
distracted_types.append(DistractedType.DISTRACTED_BLINK)
if self.ee1_calibrated:
@ -214,13 +214,7 @@ class DriverStatus():
return distracted_types
def set_policy(self, model_data, car_speed):
ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob
bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
# TODO: retune adaptive blink
self.blink.cfactor = interp(ep, [0, 0.5, 1],
[self.settings._BLINK_THRESHOLD_STRICT,
self.settings._BLINK_THRESHOLD,
self.settings._BLINK_THRESHOLD_SLACK]) / self.settings._BLINK_THRESHOLD
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
bp_normal = max(min(bp / k1, 0.5),0)
self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5],
@ -231,28 +225,36 @@ class DriverStatus():
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
def update_states(self, driver_state, cal_rpy, car_speed, op_engaged):
if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition,
driver_state.faceOrientationStd, driver_state.facePositionStd,
driver_state.readyProb, driver_state.notReadyProb)):
# rhd_pred = driver_state.wheelOnRightProb
# if car_speed > 0.01:
# self.wheelpos_learner.push_and_update(rhd_pred)
# if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT:
# self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
# else:
# self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
driver_data.faceOrientationStd, driver_data.facePositionStd,
driver_data.readyProb, driver_data.notReadyProb)):
return
self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD
self.face_detected = driver_state.faceProb > self.settings._FACE_THRESHOLD or self.face_partial
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region)
self.pose.pitch_std = driver_state.faceOrientationStd[0]
self.pose.yaw_std = driver_state.faceOrientationStd[1]
# self.pose.roll_std = driver_state.faceOrientationStd[2]
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
if self.wheel_on_right:
self.pose.yaw *= -1
self.pose.pitch_std = driver_data.faceOrientationStd[0]
self.pose.yaw_std = driver_data.faceOrientationStd[1]
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
self.eev1 = driver_state.notReadyProb[1]
self.eev2 = driver_state.readyProb[0]
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.eev1 = driver_data.notReadyProb[1]
self.eev2 = driver_data.readyProb[0]
self.distracted_types = self._get_distracted_types()
self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or
DistractedType.DISTRACTED_BLINK in self.distracted_types) and \
driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
self.driver_distraction_filter.update(self.driver_distracted)
# update offseter

24
selfdrive/monitoring/test_monitoring.py
Unescape View File

@ -17,19 +17,19 @@ INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENE
INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
def make_msg(face_detected, distracted=False, model_uncertain=False):
ds = log.DriverState.new_message()
ds.faceOrientation = [0., 0., 0.]
ds.facePosition = [0., 0.]
ds.faceProb = 1. * face_detected
ds.leftEyeProb = 1.
ds.rightEyeProb = 1.
ds.leftBlinkProb = 1. * distracted
ds.rightBlinkProb = 1. * distracted
ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
ds = log.DriverStateV2.new_message()
ds.leftDriverData.faceOrientation = [0., 0., 0.]
ds.leftDriverData.facePosition = [0., 0.]
ds.leftDriverData.faceProb = 1. * face_detected
ds.leftDriverData.leftEyeProb = 1.
ds.leftDriverData.rightEyeProb = 1.
ds.leftDriverData.leftBlinkProb = 1. * distracted
ds.leftDriverData.rightBlinkProb = 1. * distracted
ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
# TODO: test both separately when e2e is used
ds.readyProb = [0., 0., 0., 0.]
ds.notReadyProb = [0., 0.]
ds.leftDriverData.readyProb = [0., 0., 0., 0.]
ds.leftDriverData.notReadyProb = [0., 0.]
return ds

8
selfdrive/test/process_replay/model_replay.py
Unescape View File

@ -52,7 +52,7 @@ def model_replay(lr, frs):
vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size))
vipc_server.start_listener()
sm = messaging.SubMaster(['modelV2', 'driverState'])
sm = messaging.SubMaster(['modelV2', 'driverStateV2'])
pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
try:
@ -112,7 +112,7 @@ def model_replay(lr, frs):
if min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']:
recv = "modelV2"
elif msg.which() == 'driverCameraState':
recv = "driverState"
recv = "driverStateV2"
# wait for a response
with Timeout(15, f"timed out waiting for {recv}"):
@ -170,8 +170,8 @@ if __name__ == "__main__":
'logMonoTime',
'modelV2.frameDropPerc',
'modelV2.modelExecutionTime',
'driverState.modelExecutionTime',
'driverState.dspExecutionTime'
'driverStateV2.modelExecutionTime',
'driverStateV2.dspExecutionTime'
]
# TODO this tolerence is absurdly large
tolerance = 5e-1 if PC else None

2
selfdrive/test/process_replay/model_replay_ref_commit
Unescape View File

@ -1 +1 @@
629eaa7b26d1721a71547f9de880f99732cb27f3
5434b3c1696554e9a889e77f794d80cd1cb0a7ec

2
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -293,7 +293,7 @@ CONFIGS = [
ProcessConfig(
proc_name="dmonitoringd",
pub_sub={
"driverState": ["driverMonitoringState"],
"driverStateV2": ["driverMonitoringState"],
"liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
},
ignore=["logMonoTime", "valid"],

6
selfdrive/test/test_onroad.py
Unescape View File

@ -33,7 +33,7 @@ PROCS = {
"selfdrive.controls.radard": 4.5,
"./_modeld": 4.48,
"./boardd": 3.63,
"./_dmonitoringmodeld": 10.0,
"./_dmonitoringmodeld": 5.0,
"selfdrive.thermald.thermald": 3.87,
"selfdrive.locationd.calibrationd": 2.0,
"./_soundd": 1.0,
@ -60,7 +60,7 @@ TIMINGS = {
"roadCameraState": [2.5, 0.35],
"driverCameraState": [2.5, 0.35],
"modelV2": [2.5, 0.35],
"driverState": [2.5, 0.40],
"driverStateV2": [2.5, 0.40],
"liveLocationKalman": [2.5, 0.35],
"wideRoadCameraState": [1.5, 0.35],
}
@ -221,7 +221,7 @@ class TestOnroad(unittest.TestCase):
# TODO: this went up when plannerd cpu usage increased, why?
cfgs = [
("modelV2", 0.050, 0.036),
("driverState", 0.050, 0.026),
("driverStateV2", 0.050, 0.026),
]
for (s, instant_max, avg_max) in cfgs:
ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]

43
selfdrive/ui/qt/offroad/driverview.cc
Unescape View File

@ -25,7 +25,7 @@ void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) {
emit done();
}
DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) {
DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) {
face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE});
}
@ -57,43 +57,36 @@ void DriverViewScene::paintEvent(QPaintEvent* event) {
return;
}
const int width = 4 * height() / 3;
const QRect rect2 = {rect().center().x() - width / 2, rect().top(), width, rect().height()};
const QRect valid_rect = {is_rhd ? rect2.right() - rect2.height() / 2 : rect2.left(), rect2.top(), rect2.height() / 2, rect2.height()};
cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2();
cereal::DriverStateV2::DriverData::Reader driver_data;
// blackout
const QColor bg(0, 0, 0, 140);
const QRect& blackout_rect = rect();
p.fillRect(blackout_rect.adjusted(0, 0, valid_rect.left() - blackout_rect.right(), 0), bg);
p.fillRect(blackout_rect.adjusted(valid_rect.right() - blackout_rect.left(), 0, 0, 0), bg);
p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz
// is_rhd = driver_state.getWheelOnRightProb() > 0.5;
driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData();
// face bounding box
cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState();
bool face_detected = driver_state.getFaceProb() > 0.5;
bool face_detected = driver_data.getFaceProb() > 0.7;
if (face_detected) {
auto fxy_list = driver_state.getFacePosition();
auto std_list = driver_state.getFaceOrientationStd();
auto fxy_list = driver_data.getFacePosition();
auto std_list = driver_data.getFaceOrientationStd();
float face_x = fxy_list[0];
float face_y = fxy_list[1];
float face_std = std::max(std_list[0], std_list[1]);
float alpha = 0.7;
if (face_std > 0.08) {
alpha = std::max(0.7 - (face_std-0.08)*7, 0.0);
if (face_std > 0.15) {
alpha = std::max(0.7 - (face_std-0.15)*3.5, 0.0);
}
const int box_size = 0.6 * rect2.height() / 2;
const float rhd_offset = 0.05; // lhd is shifted, so rhd is not mirrored
int fbox_x = valid_rect.center().x() + (is_rhd ? (face_x + rhd_offset) : -face_x) * valid_rect.width();
int fbox_y = valid_rect.center().y() + face_y * valid_rect.height();
const int box_size = 220;
// use approx instead of distort_points
int fbox_x = 1080.0 - 1714.0 * face_x;
int fbox_y = -135.0 + (504.0 + std::abs(face_x)*112.0) + (1205.0 - std::abs(face_x)*724.0) * face_y;
p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10));
p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0);
}
// icon
const int img_offset = 30;
const int img_x = is_rhd ? rect2.right() - FACE_IMG_SIZE - img_offset : rect2.left() + img_offset;
const int img_y = rect2.bottom() - FACE_IMG_SIZE - img_offset;
p.setOpacity(face_detected ? 1.0 : 0.3);
const int img_offset = 60;
const int img_x = rect().left() + img_offset;
const int img_y = rect().bottom() - FACE_IMG_SIZE - img_offset;
p.setOpacity(face_detected ? 1.0 : 0.2);
p.drawPixmap(img_x, img_y, face_img);
}

9
selfdrive/ui/qt/widgets/cameraview.cc
Unescape View File

@ -67,16 +67,15 @@ const mat4 device_transform = {{
}};
mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) {
const float driver_view_ratio = 1.333;
const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width;
const float driver_view_ratio = 2.0;
const float yscale = stream_height * driver_view_ratio / stream_width;
const float xscale = yscale*screen_height/screen_width*stream_width/stream_height;
mat4 transform = (mat4){{
xscale, 0.0, 0.0, xscale*tici_dm_crop::x_offset/stream_width*2,
0.0, yscale, 0.0, yscale*tici_dm_crop::y_offset/stream_height*2,
xscale, 0.0, 0.0, 0.0,
0.0, yscale, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0,
}};
return transform;
}

2
system/hardware/tici/test_power_draw.py
Unescape View File

@ -21,7 +21,7 @@ class Proc:
PROCS = [
Proc('camerad', 2.15),
Proc('modeld', 1.0, atol=0.15),
Proc('dmonitoringmodeld', 0.25),
Proc('dmonitoringmodeld', 0.35),
Proc('encoderd', 0.23),
]

Write Preview
Loading…
Cancel
Save