Add Subscriber::getNumPublishers() (#1973)

core/roscpp/include/ros/subscriber.h

@@ -62,6 +62,11 @@ public:
 
   std::string getTopic() const;
 
+  /**
+   * \brief Returns the number of publishers this subscriber is connected to
+   */
+  uint32_t getNumPublishers() const;
+
   operator void*() const { return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; }
 
   bool operator<(const Subscriber& rhs) const

core/roscpp/include/ros/subscription.h

@@ -110,8 +110,9 @@ public:
    */
   void removePublisherLink(const PublisherLinkPtr& pub_link);
 
-  const std::string& getName() { return name_; }
-  uint32_t getNumCallbacks() { return callbacks_.size(); }
+  const std::string& getName() const { return name_; }
+  uint32_t getNumCallbacks() const { return callbacks_.size(); }
+  uint32_t getNumPublishers();
 
   // We'll keep a list of these objects, representing in-progress XMLRPC 
   // connections to other nodes.

core/roscpp/include/ros/topic_manager.h

@@ -106,6 +106,14 @@ public:
   size_t getNumSubscribers(const std::string &_topic);
   size_t getNumSubscriptions();
 
+  /**
+   * \brief Return the number of publishers connected to this node on a particular topic
+   *
+   * \param _topic the topic name to check
+   * \return the number of subscribers
+   */
+  size_t getNumPublishers(const std::string &_topic);
+
   /** @brief Publish a message.
    *
    * This method publishes a message on a topic, delivering it to any

core/roscpp/src/libros/subscriber.cpp

@@ -95,4 +95,14 @@ std::string Subscriber::getTopic() const
   return std::string();
 }
 
+uint32_t Subscriber::getNumPublishers() const
+{
+	if (impl_ && impl_->isValid())
+	{
+		return TopicManager::instance()->getNumPublishers(impl_->topic_);
+	}
+
+	return 0;
+}
+
 } // namespace ros

core/roscpp/src/libros/subscription.cpp

@@ -133,6 +133,12 @@ void Subscription::getInfo(XmlRpc::XmlRpcValue& info)
   }
 }
 
+uint32_t Subscription::getNumPublishers()
+{
+	boost::mutex::scoped_lock lock(publisher_links_mutex_);
+	return (uint32_t)publisher_links_.size();
+}
+
 void Subscription::drop()
 {
   if (!dropped_)

core/roscpp/src/libros/topic_manager.cpp

@@ -762,6 +762,27 @@ size_t TopicManager::getNumSubscriptions()
   return subscriptions_.size();
 }
 
+size_t TopicManager::getNumPublishers(const std::string &topic)
+{
+  boost::mutex::scoped_lock lock(subs_mutex_);
+
+  if (isShuttingDown())
+  {
+    return 0;
+  }
+
+  for (L_Subscription::const_iterator t = subscriptions_.begin();
+       t != subscriptions_.end(); ++t)
+  {
+    if ((*t)->getName() == topic)
+    {
+      return (*t)->getNumPublishers();
+    }
+  }
+
+  return 0;
+}
+
 void TopicManager::getBusStats(XmlRpcValue &stats)
 {
   XmlRpcValue publish_stats, subscribe_stats, service_stats;

test/test_roscpp/src/handles.cpp

@@ -193,6 +193,23 @@ TEST(RoscppHandles, subscriberSpinAfterSubscriberShutdown)
   ASSERT_EQ(last_fn_count, g_recv_count);
 }
 
+TEST(RoscppHandles, subscriberGetNumPublishers)
+{
+	ros::NodeHandle n;
+	ros::Publisher pub = n.advertise<test_roscpp::TestArray>("test", 0);
+
+	ros::Subscriber sub = n.subscribe("test", 0, subscriberCallback);
+
+	ros::WallTime begin = ros::WallTime::now();
+	while (sub.getNumPublishers() < 1 && (ros::WallTime::now() - begin < ros::WallDuration(1)))
+	{
+		ros::spinOnce();
+		ros::WallDuration(0.1).sleep();
+	}
+
+	ASSERT_EQ(sub.getNumPublishers(), 1ULL);
+}
+
 TEST(RoscppHandles, subscriberCopy)
 {
   ros::NodeHandle n;