Allow mixing latched and unlatched publishers.

clients/roscpp/include/ros/publication.h

@@ -58,7 +58,7 @@ class ROSCPP_DECL Publication
             const std::string& _md5sum,
             const std::string& message_definition,
             size_t max_queue,
-            bool latch,
+            bool /* unused */,
             bool has_header);
 
   ~Publication();

clients/roscpp/src/libros/publication.cpp

@@ -78,7 +78,7 @@ Publication::Publication(const std::string &name,
                          const std::string &_md5sum,
                          const std::string& message_definition,
                          size_t max_queue,
-                         bool latch,
+                         bool /* unused */,
                          bool has_header)
 : name_(name),
   datatype_(datatype),
@@ -87,7 +87,7 @@ Publication::Publication(const std::string &name,
   max_queue_(max_queue),
   seq_(0),
   dropped_(false),
-  latch_(latch),
+  latch_(false),
   has_header_(has_header),
   intraprocess_subscriber_count_(0)
 {
@@ -117,6 +117,12 @@ void Publication::addCallbacks(const SubscriberCallbacksPtr& callbacks)
       callbacks->callback_queue_->addCallback(cb, (uint64_t)callbacks.get());
     }
   }
+
+  // Publication singleton is latched if any of its callbacks have a latched message handler.
+  if (callbacks->push_latched_message_)
+  {
+    latch_ = true;
+  }
 }
 
 void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
@@ -133,6 +139,22 @@ void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
     }
     callbacks_.erase(it);
   }
+
+  // IF the removed callbacks was latched, check for remaining latched callbacks,
+  // and if none remain, clear the latch status on the publication singleton.
+  if (callbacks->push_latched_message_)
+  {
+    V_Callback::iterator it = callbacks_.begin();
+    V_Callback::iterator end = callbacks_.end();
+    for (; it != end; ++it)
+    {
+      if ((*it)->push_latched_message_)
+      {
+        return;
+      }
+    }
+    latch_ = false;
+  }
 }
 
 void Publication::drop()

clients/roscpp/src/libros/topic_manager.cpp

@@ -346,19 +346,12 @@ bool TopicManager::advertise(const AdvertiseOptions& ops, const SubscriberCallba
         return false;
       }
 
-      if (pub->isLatched() != ops.latch)
-      {
-        ROS_ERROR("Tried to advertise on topic [%s] with latch=%d but the topic is already advertised with latch=%d",
-                  ops.topic.c_str(), ops.latch, pub->isLatched());
-        return false;
-      }
-
       pub->addCallbacks(callbacks);
 
       return true;
     }
 
-    pub = PublicationPtr(boost::make_shared<Publication>(ops.topic, ops.datatype, ops.md5sum, ops.message_definition, ops.queue_size, ops.latch, ops.has_header));
+    pub = PublicationPtr(boost::make_shared<Publication>(ops.topic, ops.datatype, ops.md5sum, ops.message_definition, ops.queue_size, false, ops.has_header));
     pub->addCallbacks(callbacks);
     advertised_topics_.push_back(pub);
   }

test/test_roscpp/test/src/multiple_latched_publishers.cpp

@@ -40,41 +40,10 @@
 
 #include <gtest/gtest.h>
 #include "ros/ros.h"
+#include "ros/topic_manager.h"
 #include <std_msgs/builtin_bool.h>
 
 
-TEST(MultipleLatchedPublishers, PublisherIncompatibleAdvertise)
-{
-  {
-    ros::NodeHandle nh;
-    auto pub1 = nh.advertise<bool>("foo", 10, true);
-    auto pub2 = nh.advertise<bool>("foo", 10, false);
-    EXPECT_TRUE(pub1);
-    EXPECT_FALSE(pub2);
-  }
-  {
-    ros::NodeHandle nh;
-    auto pub1 = nh.advertise<bool>("foo", 10, false);
-    auto pub2 = nh.advertise<bool>("foo", 10, true);
-    EXPECT_TRUE(pub1);
-    EXPECT_FALSE(pub2);
-  }
-  {
-    ros::NodeHandle nh;
-    auto pub1 = nh.advertise<bool>("foo", 10, true);
-    auto pub2 = nh.advertise<bool>("foo", 10, true);
-    EXPECT_TRUE(pub1);
-    EXPECT_TRUE(pub2);
-  }
-  {
-    ros::NodeHandle nh;
-    auto pub1 = nh.advertise<bool>("foo", 10, false);
-    auto pub2 = nh.advertise<bool>("foo", 10, false);
-    EXPECT_TRUE(pub1);
-    EXPECT_TRUE(pub2);
-  }
-}
-
 TEST(MultipleLatchedPublishers, LatchedPublisherReceiveMultiple)
 {
   ros::NodeHandle nh;
@@ -83,10 +52,17 @@ TEST(MultipleLatchedPublishers, LatchedPublisherReceiveMultiple)
     nh.advertise<bool>("foo", 10, true),
     nh.advertise<bool>("foo", 10, true),
   };
+  std::vector<ros::Publisher> unlatched_publishers = {
+    nh.advertise<bool>("foo", 10, false),
+    nh.advertise<bool>("foo", 10, false),
+  };
 
   for (auto& pub : latched_publishers) {
     pub.publish(true);
   }
+  for (auto& pub : unlatched_publishers) {
+    pub.publish(true);
+  }
 
   std::size_t msg_count = 0;
   std::promise<bool> received_promise;
@@ -104,6 +80,34 @@ TEST(MultipleLatchedPublishers, LatchedPublisherReceiveMultiple)
   EXPECT_EQ(latched_publishers.size(), msg_count);
 }
 
+TEST(MultipleLatchedPublishers, TopicManagerIsLatched)
+{
+  ros::NodeHandle nh;
+  std::vector<ros::Publisher> publishers_bar = {
+    nh.advertise<bool>("bar", 10, false),
+    nh.advertise<bool>("bar", 10, true),
+    nh.advertise<bool>("bar", 10, false),
+  };
+  EXPECT_TRUE(ros::TopicManager::instance()->isLatched("/bar"));
+
+  std::vector<ros::Publisher> publishers_baz = {
+    nh.advertise<bool>("baz", 10, false),
+    nh.advertise<bool>("baz", 10, false),
+  };
+  EXPECT_FALSE(ros::TopicManager::instance()->isLatched("/baz"));
+}
+
+TEST(MultipleLatchedPublishers, TopicManagerLatchShutdown)
+{
+  ros::NodeHandle nh;
+  ros::Publisher qux_latched = nh.advertise<bool>("qux", 10, true);
+  ros::Publisher qux_unlatched = nh.advertise<bool>("qux", 10, false);
+  EXPECT_TRUE(ros::TopicManager::instance()->isLatched("/qux"));
+
+  qux_latched.shutdown();
+  EXPECT_FALSE(ros::TopicManager::instance()->isLatched("/qux"));
+}
+
 int main(int argc, char **argv) {
   ::testing::InitGoogleTest(&argc, argv);
   ros::init(argc, argv, "test_multiple_latched_publishers");