From 68d9167758b651d2e4c01f4aeb74a9613110e751 Mon Sep 17 00:00:00 2001 From: Yadu Date: Thu, 28 Dec 2023 18:12:11 +0800 Subject: [PATCH] Append measured subscription frequency to topic status (#1113) Signed-off-by: Yadunund --- .../rviz_common/message_filter_display.hpp | 20 ++++++++++++++++--- .../include/rviz_common/ros_topic_display.hpp | 18 +++++++++++++++-- .../image/image_transport_display.hpp | 18 +++++++++++++++-- .../displays/map/map_display.hpp | 1 + .../displays/map/map_display.cpp | 17 ++++++++++++++-- 5 files changed, 65 insertions(+), 9 deletions(-) diff --git a/rviz_common/include/rviz_common/message_filter_display.hpp b/rviz_common/include/rviz_common/message_filter_display.hpp index 589a83bdb..9b522e547 100644 --- a/rviz_common/include/rviz_common/message_filter_display.hpp +++ b/rviz_common/include/rviz_common/message_filter_display.hpp @@ -119,16 +119,18 @@ class MessageFilterDisplay : public _RosTopicDisplay } try { + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); subscription_ = std::make_shared>( - rviz_ros_node_.lock()->get_raw_node(), + node, topic_property_->getTopicStd(), qos_profile.get_rmw_qos_profile()); + subscription_start_time_ = node->now(); tf_filter_ = std::make_shared>( *context_->getFrameManager()->getTransformer(), fixed_frame_.toStdString(), static_cast(message_queue_property_->getInt()), - rviz_ros_node_.lock()->get_raw_node()); + node); tf_filter_->connectInput(*subscription_); tf_filter_->registerCallback( std::bind( @@ -204,10 +206,21 @@ class MessageFilterDisplay : public _RosTopicDisplay auto msg = std::static_pointer_cast(type_erased_msg); ++messages_received_; + QString topic_str = QString::number(messages_received_) + " messages received"; + // Append topic subscription frequency if we can lock rviz_ros_node_. + std::shared_ptr node_interface = + rviz_ros_node_.lock(); + if (node_interface != nullptr) { + const double duration = + (node_interface->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( properties::StatusProperty::Ok, "Topic", - QString::number(messages_received_) + " messages received"); + topic_str); processMessage(msg); } @@ -219,6 +232,7 @@ class MessageFilterDisplay : public _RosTopicDisplay virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0; typename std::shared_ptr> subscription_; + rclcpp::Time subscription_start_time_; std::shared_ptr> tf_filter_; uint32_t messages_received_; properties::IntProperty * message_queue_property_; diff --git a/rviz_common/include/rviz_common/ros_topic_display.hpp b/rviz_common/include/rviz_common/ros_topic_display.hpp index e28c60a44..e728ff451 100644 --- a/rviz_common/include/rviz_common/ros_topic_display.hpp +++ b/rviz_common/include/rviz_common/ros_topic_display.hpp @@ -215,12 +215,14 @@ class RosTopicDisplay : public _RosTopicDisplay }; // TODO(anhosi,wjwwood): replace with abstraction for subscriptions once available + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); subscription_ = - rviz_ros_node_.lock()->get_raw_node()->template create_subscription( + node->template create_subscription( topic_property_->getTopicStd(), qos_profile, [this](const typename MessageType::ConstSharedPtr message) {incomingMessage(message);}, sub_opts); + subscription_start_time_ = node->now(); setStatus(properties::StatusProperty::Ok, "Topic", "OK"); } catch (rclcpp::exceptions::InvalidTopicNameError & e) { setStatus( @@ -260,10 +262,21 @@ class RosTopicDisplay : public _RosTopicDisplay } ++messages_received_; + QString topic_str = QString::number(messages_received_) + " messages received"; + // Append topic subscription frequency if we can lock rviz_ros_node_. + std::shared_ptr node_interface = + rviz_ros_node_.lock(); + if (node_interface != nullptr) { + const double duration = + (node_interface->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( properties::StatusProperty::Ok, "Topic", - QString::number(messages_received_) + " messages received"); + topic_str); processMessage(msg); } @@ -274,6 +287,7 @@ class RosTopicDisplay : public _RosTopicDisplay virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0; typename rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Time subscription_start_time_; uint32_t messages_received_; }; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp index a3638007b..2d20735c6 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp @@ -111,11 +111,13 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay try { subscription_ = std::make_shared(); + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); subscription_->subscribe( - rviz_ros_node_.lock()->get_raw_node().get(), + node.get(), getBaseTopicFromTopic(topic_property_->getTopicStd()), getTransportFromTopic(topic_property_->getTopicStd()), qos_profile.get_rmw_qos_profile()); + subscription_start_time_ = node->now(); subscription_callback_ = subscription_->registerCallback( std::bind( &ImageTransportDisplay::incomingMessage, this, std::placeholders::_1)); @@ -169,10 +171,21 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay } ++messages_received_; + QString topic_str = QString::number(messages_received_) + " messages received"; + // Append topic subscription frequency if we can lock rviz_ros_node_. + std::shared_ptr node_interface = + rviz_ros_node_.lock(); + if (node_interface != nullptr) { + const double duration = + (node_interface->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, "Topic", - QString::number(messages_received_) + " messages received"); + topic_str); processMessage(msg); } @@ -187,6 +200,7 @@ class ImageTransportDisplay : public rviz_common::_RosTopicDisplay uint32_t messages_received_; std::shared_ptr subscription_; + rclcpp::Time subscription_start_time_; message_filters::Connection subscription_callback_; }; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp index dc55d6c58..e4c0c3d78 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp @@ -174,6 +174,7 @@ protected Q_SLOTS: rclcpp::Subscription::SharedPtr update_subscription_; rclcpp::QoS update_profile_; + rclcpp::Time subscription_start_time_; rviz_common::properties::RosTopicProperty * update_topic_property_; rviz_common::properties::QosProfileProperty * update_profile_property_; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp index f785efe5e..ad1a553f7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp @@ -260,14 +260,16 @@ void MapDisplay::subscribeToUpdateTopic() QString(sstm.str().c_str())); }; + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); update_subscription_ = - rviz_ros_node_.lock()->get_raw_node()-> + node-> template create_subscription( update_topic_property_->getTopicStd(), update_profile_, [this](const map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr message) { incomingUpdate(message); }, sub_opts); + subscription_start_time_ = node->now(); setStatus(rviz_common::properties::StatusProperty::Ok, "Update Topic", "OK"); } catch (rclcpp::exceptions::InvalidTopicNameError & e) { setStatus( @@ -356,10 +358,21 @@ void MapDisplay::incomingUpdate(const map_msgs::msg::OccupancyGridUpdate::ConstS } ++update_messages_received_; + QString topic_str = QString::number(messages_received_) + " update messages received"; + // Append topic subscription frequency if we can lock rviz_ros_node_. + std::shared_ptr node_interface = + rviz_ros_node_.lock(); + if (node_interface != nullptr) { + const double duration = + (node_interface->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, "Topic", - QString::number(update_messages_received_) + " update messages received"); + topic_str); if (updateDataOutOfBounds(update)) { setStatus(