From d4e8ed70f369543e9bfc057a46ecf539283ef3d6 Mon Sep 17 00:00:00 2001
From: Enrique Fernandez <efernandez@clearpath.ai>
Date: Fri, 27 Apr 2018 14:35:36 -0400
Subject: [PATCH] Add hasSubscribers helper method

This simplifies the connectCallback code.
---
 .../nodelet_topic_tools/nodelet_lazy.h        | 70 ++++++++++---------
 1 file changed, 37 insertions(+), 33 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 c5b0152c..b1b19e3f 100644
--- a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
+++ b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_lazy.h
@@ -146,47 +146,23 @@ class NodeletLazy: public nodelet::Nodelet
     if (lazy_)
     {
       boost::mutex::scoped_lock lock(connection_mutex_);
-      BOOST_FOREACH (const ros::Publisher& pub, publishers_)
+      if (hasSubscribers())
       {
-        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;
         }
-      }
-      BOOST_FOREACH (const image_transport::Publisher& pub, image_transport_publishers_)
-      {
-        if (pub.getNumSubscribers() > 0)
+        if (!ever_subscribed_)
         {
-          if (connection_status_ != SUBSCRIBED)
-          {
-            if (verbose_connection_)
-            {
-              NODELET_INFO("Subscribe input topics");
-            }
-            subscribe();
-            connection_status_ = SUBSCRIBED;
-          }
-          if (!ever_subscribed_)
-          {
-            ever_subscribed_ = true;
-          }
-          return;
+          ever_subscribed_ = true;
         }
       }
-      if (connection_status_ == SUBSCRIBED)
+      else if (connection_status_ == SUBSCRIBED)
       {
         if (verbose_connection_)
         {
@@ -279,6 +255,34 @@ class NodeletLazy: public nodelet::Nodelet
     return pub;
   }
 
+  /**
+   * @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 hasSubscribers(const std::vector<T>& publishers) const
+  {
+    BOOST_FOREACH (const T& publisher, publishers)
+    {
+      if (publisher.getNumSubscribers() > 0)
+      {
+        return true;
+      }
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Checkes if at least one ros::Publisher or image_transport::Publisher has subscribers.
+   * @return True if at least one publisher has subscribers; False otherwise.
+   */
+  bool hasSubscribers() const
+  {
+    return hasSubscribers(publishers_) || hasSubscribers(image_transport_publishers_);
+  }
+
   /** @brief
     * mutex to call subscribe() and unsubscribe() in
     * critical section.