From ea2dcf9d37363f95d890fc96f68c2184811b7065 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 14 Dec 2021 17:04:40 -0800 Subject: [PATCH] modeld: start publishing before calib seen (#23190) old-commit-hash: fa62b9d3f9882ec33559ceb7377fa991be1d4fb5 --- selfdrive/modeld/modeld.cc | 54 ++++++++++++++---------------- selfdrive/modeld/models/driving.cc | 8 ++--- selfdrive/modeld/models/driving.h | 4 +-- 3 files changed, 31 insertions(+), 35 deletions(-) diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 9e1750eeaf..a682fb6349 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -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; - } + 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.; - } + 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); + 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(model.output.data(), model.output.size())); - posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof); + model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, extra.timestamp_eof, model_execution_time, + kj::ArrayPtr(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; - } + //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; } } diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 629e1d7ed1..22d0716f67 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -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 raw_pred) { + float model_execution_time, kj::ArrayPtr 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)}); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 5749fb5ec2..115e1342c1 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -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 raw_pred); + float model_execution_time, kj::ArrayPtr 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);