publish processing rate (topic process_hz)

ov_msckf/src/core/VioManager.cpp

@@ -161,6 +161,7 @@ VioManager::VioManager(VioManagerOptions &params_) : thread_init_running(false),
                                                         propagator, params.gravity_mag, params.zupt_max_velocity,
                                                         params.zupt_noise_multiplier, params.zupt_max_disparity);
   }
+
 }
 
 void VioManager::feed_measurement_imu(const ov_core::ImuData &message) {
@@ -711,4 +712,5 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
                state->_calib_imu_tg->value()(4), state->_calib_imu_tg->value()(5), state->_calib_imu_tg->value()(6),
                state->_calib_imu_tg->value()(7), state->_calib_imu_tg->value()(8));
   }
+  num_imgs_processed++;
 }

ov_msckf/src/core/VioManager.h

@@ -122,6 +122,8 @@ class VioManager {
   /// Returns 3d features used in the last update in global frame
   std::vector<Eigen::Vector3d> get_good_features_MSCKF() { return good_features_MSCKF; }
 
+  int num_imgs_processed = 0;
+
   /// Return the image used when projecting the active tracks
   void get_active_image(double &timestamp, cv::Mat &image) {
     timestamp = active_tracks_time;

ov_msckf/src/ros/ROS1Visualizer.cpp

@@ -80,6 +80,8 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
   it_pub_loop_img_depth = it.advertise("loop_depth", 2);
   it_pub_loop_img_depth_color = it.advertise("loop_depth_colored", 2);
 
+  pub_process_hz = nh->advertise<std_msgs::Float64>("process_hz", 2);
+
   // option to enable publishing of global to IMU transformation
   nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true);
   nh->param<bool>("publish_calibration_tf", publish_calibration_tf, true);
@@ -145,6 +147,24 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
       }
     });
     thread.detach();
+
+    std::thread thread_pub_hz([&] {
+      ros::Rate loop_rate(1);
+      while (ros::ok()) {
+        if (_app->num_imgs_processed > 0) {
+          boost::posix_time::ptime t_now = boost::posix_time::microsec_clock::local_time();
+          double dt = (t_now - t_pub_hz_last).total_microseconds() * 1e-6;
+          double avg_hz = _app->num_imgs_processed /dt;
+          t_pub_hz_last = t_now;
+          _app->num_imgs_processed = 0;
+          std_msgs::Float64 msg_hz;
+          msg_hz.data = avg_hz;
+          pub_process_hz.publish(msg_hz);
+          loop_rate.sleep();
+        }
+      }
+    });
+    thread_pub_hz.detach();
   }
 }
 

ov_msckf/src/ros/ROS1Visualizer.h

@@ -144,6 +144,7 @@ class ROS1Visualizer {
   ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu;
   ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim;
   ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics;
+  ros::Publisher pub_process_hz;
   std::shared_ptr<tf::TransformBroadcaster> mTfBr;
 
   // Our subscribers and camera synchronizers
@@ -167,7 +168,7 @@ class ROS1Visualizer {
 
   // Start and end timestamps
   bool start_time_set = false;
-  boost::posix_time::ptime rT1, rT2;
+  boost::posix_time::ptime rT1, rT2, t_pub_hz_last;
 
   // Thread atomics
   std::atomic<bool> thread_update_running;