From ad1990bfa180f39b4cf04116438453783bb125f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Feb 2024 18:10:57 +0100 Subject: [PATCH] DepthCloud plugin: Append measured subscription frequency to topic status (#1137) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../depth_cloud/depth_cloud_display.hpp | 1 + .../depth_cloud/depth_cloud_display.cpp | 21 ++++++++++++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp index 5ae8a2f53..043816345 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp @@ -155,6 +155,7 @@ protected Q_SLOTS: typedef message_filters::Synchronizer SynchronizerDepthColor; std::shared_ptr 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 index 9958c68f1..7ecfb1097 100644 --- 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(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; {