Append measured subscription frequency to topic status (ros-visualiza…

…tion#1113) (ros-visualization#1129)

Signed-off-by: Yadunund <yadunund@openrobotics.org>
(cherry picked from commit 68d9167758b651d2e4c01f4aeb74a9613110e751)

Co-authored-by: Yadu <yadunund@openrobotics.org>

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<message_filters::Subscriber<MessageType>>(
-        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<tf2_ros::MessageFilter<MessageType, transformation::FrameTransformer>>(
         *context_->getFrameManager()->getTransformer(),
         fixed_frame_.toStdString(),
         static_cast<uint32_t>(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<const MessageType>(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<ros_integration::RosNodeAbstractionIface> 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<double>(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<message_filters::Subscriber<MessageType>> subscription_;
+  rclcpp::Time subscription_start_time_;
   std::shared_ptr<tf2_ros::MessageFilter<MessageType, transformation::FrameTransformer>> tf_filter_;
   uint32_t messages_received_;
   properties::IntProperty * message_queue_property_;

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<MessageType>(
+        node->template create_subscription<MessageType>(
         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<ros_integration::RosNodeAbstractionIface> 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<double>(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<MessageType>::SharedPtr subscription_;
+  rclcpp::Time subscription_start_time_;
   uint32_t messages_received_;
 };
 

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<image_transport::SubscriberFilter>();
+      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<MessageType>::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<rviz_common::ros_integration::RosNodeAbstractionIface> 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<double>(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<image_transport::SubscriberFilter> subscription_;
+  rclcpp::Time subscription_start_time_;
   message_filters::Connection subscription_callback_;
 };
 

rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp

@@ -173,6 +173,7 @@ protected Q_SLOTS:
 
   rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::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_;

rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp

@@ -232,14 +232,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<map_msgs::msg::OccupancyGridUpdate>(
       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(
@@ -328,10 +330,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<rviz_common::ros_integration::RosNodeAbstractionIface> 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<double>(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(