spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

modeld: start publishing before calib seen (#23190)

pull/23225/head
Adeeb Shihadeh 2021-12-14 17:04:40 -08:00 committed by GitHub
parent 245b4d910c
commit fa62b9d3f9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 34 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
selfdrive/modeld/modeld.cc
View File

@ -87,7 +87,6 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
transform_lock.lock();
mat3 model_transform = cur_transform;
const bool run_model_this_iter = live_calib_seen;
transform_lock.unlock();
// TODO: path planner timeout?
@ -95,38 +94,35 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
int 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();
ModelOutput *model_output = 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;
float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
if (run_count < 10) { // let frame drops warm up
frame_dropped_filter.reset(0);
frames_dropped = 0.;
}
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, 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_output, extra.timestamp_eof);
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
last_vipc_frame_id = extra.frame_id;
float vec_desire[DESIRE_LEN] = {0};
if (desire >= 0 && desire < DESIRE_LEN) {
vec_desire[desire] = 1.0;
}
double mt1 = millis_since_boot();
ModelOutput *model_output = 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;
float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
if (run_count < 10) { // let frame drops warm up
frame_dropped_filter.reset(0);
frames_dropped = 0.;
}
run_count++;
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, extra.timestamp_eof, model_execution_time,
kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof, live_calib_seen);
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
last_vipc_frame_id = extra.frame_id;
}
}

8
selfdrive/modeld/models/driving.cc
View File

@ -308,10 +308,10 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred) {
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg;
auto framed = msg.initEvent().initModelV2();
auto framed = msg.initEvent(valid).initModelV2();
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
@ -325,14 +325,14 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
}
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelOutput &net_outputs, uint64_t timestamp_eof) {
const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid) {
MessageBuilder msg;
auto v_mean = net_outputs.pose.velocity_mean;
auto r_mean = net_outputs.pose.rotation_mean;
auto v_std = net_outputs.pose.velocity_std;
auto r_std = net_outputs.pose.rotation_std;
auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});

4
selfdrive/modeld/models/driving.h
View File

@ -238,6 +238,6 @@ void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred);
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelOutput &net_outputs, uint64_t timestamp_eof);
const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);