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