Only subscribe to /gazebo/performance_metrics when necessary (#1209)

Backport of #1202 to kinetic-level.

We are currently subscribing to the /gazebo/performance_metrics topic
even if there are no subscribers to the ROS topic forwarding this data.
The link_states and model_states topics currently use an advertise
mechanism with callbacks when a subscriber connects or disconnects,
so I've used that same pattern for the performance_metrics topic.
This also helps workaround the deadlock documented in #1175 and
gazebosim/gazebo-classic#2902.

This also adds a GAZEBO_ROS_HAS_PERFORMANCE_METRICS
macro that reduces duplication of the version checking logic for
performance metrics in gazebo and adds fixes some doc-string and
typos in existing code

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

Author: scpeters

gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h

@@ -101,6 +101,13 @@
 
 #include <boost/algorithm/string.hpp>
 
+#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
 {
 
@@ -131,18 +138,26 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief advertise services
   void advertiseServices();
 
-  /// \brief
+  /// \brief Callback for a subscriber connecting to LinkStates ros topic.
   void onLinkStatesConnect();
 
-  /// \brief
+  /// \brief Callback for a subscriber connecting to ModelStates ros topic.
   void onModelStatesConnect();
 
-  /// \brief
+  /// \brief Callback for a subscriber disconnecting from LinkStates ros topic.
   void onLinkStatesDisconnect();
 
-  /// \brief
+  /// \brief Callback for a subscriber disconnecting from ModelStates ros topic.
   void onModelStatesDisconnect();
 
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+  /// \brief Callback for a subscriber connecting to PerformanceMetrics ros topic.
+  void onPerformanceMetricsConnect();
+
+  /// \brief Callback for a subscriber disconnecting from PerformanceMetrics ros topic.
+  void onPerformanceMetricsDisconnect();
+#endif
+
   /// \brief Function for inserting a URDF into Gazebo from ROS Service Call
   bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
                       gazebo_msgs::SpawnModel::Response &res);
@@ -292,7 +307,7 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief Unused
   void onResponse(ConstResponsePtr &response);
 
-#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
   void onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg);
 #endif
@@ -375,6 +390,7 @@ class GazeboRosApiPlugin : public SystemPlugin
   ros::Publisher     pub_performance_metrics_;
   int                pub_link_states_connection_count_;
   int                pub_model_states_connection_count_;
+  int                pub_performance_metrics_connection_count_;
 
   // ROS comm
   boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;

gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -32,6 +32,7 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
   stop_(false),
   pub_link_states_connection_count_(0),
   pub_model_states_connection_count_(0),
+  pub_performance_metrics_connection_count_(0),
   physics_reconfigure_initialized_(false),
   pub_clock_frequency_(0),
   world_created_(false)
@@ -187,13 +188,10 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
   light_modify_pub_ = gazebonode_->Advertise<gazebo::msgs::Light>("~/light/modify");
   request_pub_ = gazebonode_->Advertise<gazebo::msgs::Request>("~/request");
   response_sub_ = gazebonode_->Subscribe("~/response",&GazeboRosApiPlugin::onResponse, this);
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
-  performance_metric_sub_ = gazebonode_->Subscribe("/gazebo/performance_metrics",
-    &GazeboRosApiPlugin::onPerformanceMetrics, this);
-#endif
   // reset topic connection counts
   pub_link_states_connection_count_ = 0;
   pub_model_states_connection_count_ = 0;
+  pub_performance_metrics_connection_count_ = 0;
 
   /// \brief advertise all services
   advertiseServices();
@@ -208,7 +206,7 @@ void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
 {
 
 }
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
 void GazeboRosApiPlugin::onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg)
 {
   gazebo_msgs::PerformanceMetrics msg_ros;
@@ -385,9 +383,15 @@ void GazeboRosApiPlugin::advertiseServices()
                                                             ros::VoidPtr(), &gazebo_queue_);
   pub_model_states_ = nh_->advertise(pub_model_states_ao);
 
-#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
   // publish performance metrics
-  pub_performance_metrics_ = nh_->advertise<gazebo_msgs::PerformanceMetrics>("performance_metrics", 10);
+  ros::AdvertiseOptions pub_performance_metrics_ao =
+    ros::AdvertiseOptions::create<gazebo_msgs::PerformanceMetrics>(
+                                                                   "performance_metrics",10,
+                                                                   boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsConnect,this),
+                                                                   boost::bind(&GazeboRosApiPlugin::onPerformanceMetricsDisconnect,this),
+                                                                   ros::VoidPtr(), &gazebo_queue_);
+  pub_performance_metrics_ = nh_->advertise(pub_performance_metrics_ao);
 #endif
 
   // Advertise more services on the custom queue
@@ -561,14 +565,37 @@ void GazeboRosApiPlugin::onModelStatesConnect()
     pub_model_states_event_   = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates,this));
 }
 
+#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
+void GazeboRosApiPlugin::onPerformanceMetricsConnect()
+{
+  pub_performance_metrics_connection_count_++;
+  if (pub_performance_metrics_connection_count_ == 1) // connect on first subscriber
+  {
+    performance_metric_sub_ = gazebonode_->Subscribe("/gazebo/performance_metrics",
+      &GazeboRosApiPlugin::onPerformanceMetrics, this);
+  }
+}
+
+void GazeboRosApiPlugin::onPerformanceMetricsDisconnect()
+{
+  pub_performance_metrics_connection_count_--;
+  if (pub_performance_metrics_connection_count_ <= 0) // disconnect with no subscribers
+  {
+    performance_metric_sub_.reset();
+    if (pub_performance_metrics_connection_count_ < 0) // should not be possible
+      ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_performance_metrics_ in gazebo_ros.cpp? something weird");
+  }
+}
+#endif
+
 void GazeboRosApiPlugin::onLinkStatesDisconnect()
 {
   pub_link_states_connection_count_--;
   if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers
   {
     pub_link_states_event_.reset();
     if (pub_link_states_connection_count_ < 0) // should not be possible
-      ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
+      ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_link_states_ in gazebo_ros.cpp? something weird");
   }
 }
 
@@ -579,7 +606,7 @@ void GazeboRosApiPlugin::onModelStatesDisconnect()
   {
     pub_model_states_event_.reset();
     if (pub_model_states_connection_count_ < 0) // should not be possible
-      ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
+      ROS_ERROR_NAMED("api_plugin", "One too many disconnect from pub_model_states_ in gazebo_ros.cpp? something weird");
   }
 }