modeld: start publishing before calib seen (#23190)

old-commit-hash: fa62b9d3f9

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,9 +94,6 @@ 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;
@@ -116,19 +112,19 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
       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()));
-      posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof);
+                  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;
   }
 }
-}
 
 int main(int argc, char **argv) {
   if (!Hardware::PC()) {

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<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)});

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<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);