ros2: Only subscribe to /gazebo/performance_metrics when necessary (#1205)

We are currently subscribing to the /gazebo/performance_metrics topic
even if there are no subscribers to the ROS topic forwarding this data.
This changes gazebo_ros_init to only subscribe to the gazebo topic
if there are any subscribers to the corresponding ROS topic.
While advertiser callbacks are used in ROS 1 but are not yet in ROS2,
here we use polling in the GazeboRosInitPrivate::PublishSimTime
callback to check for subscribers since it is called for each Gazebo
time step.

This also helps workaround the deadlock documented in #1175 and
gazebosim/gazebo-classic#2902.

This also adds a macro to reduce duplication of the version checking
logic.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Author: scpeters

gazebo_ros/src/gazebo_ros_init.cpp

@@ -33,6 +33,14 @@
 #include <memory>
 #include <string>
 
+#ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+#if \
+  (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || \
+  (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+#define GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+#endif
+#endif  // ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+
 namespace gazebo_ros
 {
 
@@ -78,8 +86,7 @@ class GazeboRosInitPrivate
     std_srvs::srv::Empty::Request::SharedPtr req,
     std_srvs::srv::Empty::Response::SharedPtr res);
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
   /// \brief Subscriber callback for performance metrics. This will be send in the ROS network
   /// \param[in] msg Received PerformanceMetrics message
   void onPerformanceMetrics(ConstPerformanceMetricsPtr & msg);
@@ -157,8 +164,7 @@ void GazeboRosInit::Load(int argc, char ** argv)
     "/clock",
     rclcpp::QoS(rclcpp::KeepLast(10)).transient_local());
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
   impl_->performance_metrics_pub_ =
     impl_->ros_node_->create_publisher<gazebo_msgs::msg::PerformanceMetrics>(
     "performance_metrics", 10);
@@ -178,8 +184,7 @@ void GazeboRosInit::Load(int argc, char ** argv)
     std::bind(&GazeboRosInitPrivate::OnWorldCreated, impl_.get(), std::placeholders::_1));
 }
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
 void GazeboRosInitPrivate::onPerformanceMetrics(
   ConstPerformanceMetricsPtr & msg)
 {
@@ -238,14 +243,10 @@ void GazeboRosInitPrivate::OnWorldCreated(const std::string & _world_name)
       &GazeboRosInitPrivate::OnUnpause, this,
       std::placeholders::_1, std::placeholders::_2));
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && \
-  GAZEBO_MINOR_VERSION > 14)
-  // Gazebo transport
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+  // Initialize gazebo transport node
   gz_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
   gz_node_->Init(world_->Name());
-  performance_metric_sub_ = gz_node_->Subscribe(
-    "/gazebo/performance_metrics",
-    &GazeboRosInitPrivate::onPerformanceMetrics, this);
 #endif
 }
 
@@ -263,6 +264,18 @@ void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _in
   rosgraph_msgs::msg::Clock clock;
   clock.clock = gazebo_ros::Convert<builtin_interfaces::msg::Time>(_info.simTime);
   clock_pub_->publish(clock);
+
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+  if (!performance_metric_sub_ && performance_metrics_pub_->get_subscription_count() > 0) {
+    // Subscribe to gazebo performance_metrics topic if there are ros subscribers
+    performance_metric_sub_ = gz_node_->Subscribe(
+      "/gazebo/performance_metrics",
+      &GazeboRosInitPrivate::onPerformanceMetrics, this);
+  } else if (performance_metric_sub_ && performance_metrics_pub_->get_subscription_count() == 0) {
+    // Unsubscribe from gazebo performance_metrics topic if there are no more ros subscribers
+    performance_metric_sub_.reset();
+  }
+#endif
 }
 
 void GazeboRosInitPrivate::OnResetSimulation(