DepthCloud plugin: Append measured subscription frequency to topic st…

…atus (#1137) Signed-off-by: Alejandro Hernández Cordero <[email protected]>
ros2 · Feb 20, 2024 · ad1990b · ad1990b
1 parent fdf1957
commit ad1990b
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 5 deletions.
diff --git a/...default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp b/...default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
@@ -155,6 +155,7 @@ protected Q_SLOTS:
   typedef message_filters::Synchronizer<SyncPolicyDepthColor> SynchronizerDepthColor;
 
   std::shared_ptr<SynchronizerDepthColor> sync_depth_color_;
+  rclcpp::Time subscription_start_time_;
 
   // RVIZ properties
   rviz_common::properties::Property * topic_filter_property_;

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
@@ -412,11 +412,22 @@ void DepthCloudDisplay::processMessage(
 
   std::ostringstream s;
 
-  ++messages_received_;
-  setStatus(
-    rviz_common::properties::StatusProperty::Ok, "Depth Map",
-    QString::number(messages_received_) + " depth maps received");
-  setStatus(rviz_common::properties::StatusProperty::Ok, "Message", "Ok");
+  {
+    ++messages_received_;
+    auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock();
+    QString topic_str = QString::number(messages_received_) + " messages received";
+    // Append topic subscription frequency if we can lock rviz_ros_node_.
+    if (rviz_ros_node_ != nullptr) {
+      const double duration =
+        (rviz_ros_node_->get_raw_node()->now() - subscription_start_time_).seconds();
+      const double subscription_frequency =
+        static_cast<double>(messages_received_) / duration;
+      topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
+    }
+    setStatus(
+      rviz_common::properties::StatusProperty::Ok, "Depth Map", topic_str);
+    setStatus(rviz_common::properties::StatusProperty::Ok, "Message", "Ok");
+  }
 
   sensor_msgs::msg::CameraInfo::ConstSharedPtr cam_info;
   {