From b13b6e503db1b56ebb56057f497a68360e8ed686 Mon Sep 17 00:00:00 2001
From: Enrique Fernandez <efernandez@clearpath.ai>
Date: Fri, 27 Apr 2018 16:52:22 -0400
Subject: [PATCH 1/2] Allow to extend w/ support for image_transport

Add hasSubscribers and remove not-needed argument from
connectionCallback, so we can overload the hasSubscriber
check and support image_transport::Publisher.

Also share connect and disconnect cb.
---
 .../nodelet_topic_tools/nodelet_lazy.h        | 64 +++++++++++++------
 1 file changed, 43 insertions(+), 21 deletions(-)

diff --git a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
index fccd942e..eb5ce69d 100644
--- a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
+++ b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
@@ -40,6 +40,7 @@
 
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
+#include <boost/foreach.hpp>
 #include <boost/thread.hpp>
 #include <string>
 #include <vector>
@@ -57,6 +58,25 @@ enum ConnectionStatus
   SUBSCRIBED
 };
 
+/**
+ * @brief Checks if at least one publisher has subscribers.
+ * @param publishers Vector of publishers.
+ * @return True if at least one publisher has subscribers; False otherwise.
+ */
+template <class T>
+bool has_subscribers(const std::vector<T>& publishers)
+{
+  BOOST_FOREACH (const T& publisher, publishers)
+  {
+    if (publisher.getNumSubscribers() > 0)
+    {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 /** @brief
   * Nodelet to automatically subscribe/unsubscribe
   * topics according to subscription of advertised topics.
@@ -135,7 +155,7 @@ class NodeletLazy: public nodelet::Nodelet
   /** @brief
     * callback function which is called when new subscriber come
     */
-  virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub)
+  virtual void connectionCallback()
   {
     if (verbose_connection_)
     {
@@ -144,28 +164,23 @@ class NodeletLazy: public nodelet::Nodelet
     if (lazy_)
     {
       boost::mutex::scoped_lock lock(connection_mutex_);
-      for (size_t i = 0; i < publishers_.size(); i++)
+      if (hasSubscribers())
       {
-        ros::Publisher pub = publishers_[i];
-        if (pub.getNumSubscribers() > 0)
+        if (connection_status_ != SUBSCRIBED)
         {
-          if (connection_status_ != SUBSCRIBED)
+          if (verbose_connection_)
           {
-            if (verbose_connection_)
-            {
-              NODELET_INFO("Subscribe input topics");
-            }
-            subscribe();
-            connection_status_ = SUBSCRIBED;
+            NODELET_INFO("Subscribe input topics");
           }
-          if (!ever_subscribed_)
-          {
-            ever_subscribed_ = true;
-          }
-          return;
+          subscribe();
+          connection_status_ = SUBSCRIBED;
+        }
+        if (!ever_subscribed_)
+        {
+          ever_subscribed_ = true;
         }
       }
-      if (connection_status_ == SUBSCRIBED)
+      else if (connection_status_ == SUBSCRIBED)
       {
         if (verbose_connection_)
         {
@@ -221,18 +236,25 @@ class NodeletLazy: public nodelet::Nodelet
   {
     boost::mutex::scoped_lock lock(connection_mutex_);
     ros::SubscriberStatusCallback connect_cb
-      = boost::bind(&NodeletLazy::connectionCallback, this, _1);
-    ros::SubscriberStatusCallback disconnect_cb
-      = boost::bind(&NodeletLazy::connectionCallback, this, _1);
+      = boost::bind(&NodeletLazy::connectionCallback, this);
     ros::Publisher pub = nh.advertise<T>(topic, queue_size,
                                           connect_cb,
-                                          disconnect_cb,
+                                          connect_cb,
                                           ros::VoidConstPtr(),
                                           latch);
     publishers_.push_back(pub);
     return pub;
   }
 
+  /**
+   * @brief Checks if at least one publisher has subscribers.
+   * @return True if at least one publisher has subscribers; False otherwise.
+   */
+  virtual bool hasSubscribers() const
+  {
+    return has_subscribers(publishers_);
+  }
+
   /** @brief
     * mutex to call subscribe() and unsubscribe() in
     * critical section.

From 52a66c630341ae2b263b17c8ddc0a3786227558b Mon Sep 17 00:00:00 2001
From: Enrique Fernandez <efernandez@clearpath.ai>
Date: Fri, 27 Apr 2018 16:54:12 -0400
Subject: [PATCH 2/2] Use const reference for std::string

---
 nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
index eb5ce69d..3cd0f95b 100644
--- a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
+++ b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
@@ -232,7 +232,7 @@ class NodeletLazy: public nodelet::Nodelet
     */
   template<class T> ros::Publisher
   advertise(ros::NodeHandle& nh,
-            std::string topic, int queue_size, bool latch=false)
+            const std::string& topic, int queue_size, bool latch=false)
   {
     boost::mutex::scoped_lock lock(connection_mutex_);
     ros::SubscriberStatusCallback connect_cb