From b441122fde4ec9baf0a75b6121eef067018b3d59 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 11 Dec 2020 17:44:50 -0800
Subject: [PATCH] Subscribe to gazebo topic on ROS topic connection

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
osrf/gazebo#2902.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 .../gazebo_ros/gazebo_ros_api_plugin.h        |  9 +++++
 gazebo_ros/src/gazebo_ros_api_plugin.cpp      | 37 ++++++++++++++++---
 2 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
index 8ef2a981e..40396ff0d 100644
--- a/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
+++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
@@ -150,6 +150,14 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \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);
@@ -383,6 +391,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_;
diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp
index b635385f2..1eaa7bc0f 100644
--- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp
+++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp
@@ -36,6 +36,7 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
   plugin_loaded_(false),
   pub_link_states_connection_count_(0),
   pub_model_states_connection_count_(0),
+  pub_performance_metrics_connection_count_(0),
   pub_clock_frequency_(0),
   enable_ros_network_(true)
 {
@@ -192,13 +193,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);
-#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
-  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;
 
   // Manage clock for simulated ros time
   pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock", 10);
@@ -410,7 +408,13 @@ void GazeboRosApiPlugin::advertiseServices()
 
 #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
@@ -571,6 +575,29 @@ 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_--;