From 8eec2319b24fdf4606187f5cfb25e897dac5b3c1 Mon Sep 17 00:00:00 2001
From: Mike Purvis <mpurvis@clearpath.ai>
Date: Sat, 11 Jul 2020 02:51:30 +0000
Subject: [PATCH] Fix isLatched behaviour, add tests.

---
 clients/roscpp/include/ros/publication.h      |  2 +-
 clients/roscpp/src/libros/publication.cpp     | 27 +++++++++++++++--
 .../test/src/multiple_latched_publishers.cpp  | 29 +++++++++++++++++++
 3 files changed, 55 insertions(+), 3 deletions(-)

diff --git a/clients/roscpp/include/ros/publication.h b/clients/roscpp/include/ros/publication.h
index 8e500f9faa..52b6f1c513 100644
--- a/clients/roscpp/include/ros/publication.h
+++ b/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();
diff --git a/clients/roscpp/src/libros/publication.cpp b/clients/roscpp/src/libros/publication.cpp
index f80e0ad4c5..4a83085101 100644
--- a/clients/roscpp/src/libros/publication.cpp
+++ b/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,13 @@ void Publication::addCallbacks(const SubscriberCallbacksPtr& callbacks)
       callbacks->callback_queue_->addCallback(cb, (uint64_t)callbacks.get());
     }
   }
+
+  // Mark the publication as having one or more latched messages if any of the callbacks associated with
+  // it have a handler defined to push them out.
+  if (callbacks->push_latched_message_)
+  {
+    latch_ = true;
+  }
 }
 
 void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
@@ -133,6 +140,22 @@ void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
     }
     callbacks_.erase(it);
   }
+
+  // IF the callbacks we're removing were latched, check the remainder of the list
+  // to see if any latched callbacks remain. If not, clear the latch status on the 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()
diff --git a/test/test_roscpp/test/src/multiple_latched_publishers.cpp b/test/test_roscpp/test/src/multiple_latched_publishers.cpp
index 785967e30e..dbd4df27f3 100644
--- a/test/test_roscpp/test/src/multiple_latched_publishers.cpp
+++ b/test/test_roscpp/test/src/multiple_latched_publishers.cpp
@@ -40,6 +40,7 @@
 
 #include <gtest/gtest.h>
 #include "ros/ros.h"
+#include "ros/topic_manager.h"
 #include <std_msgs/builtin_bool.h>
 
 
@@ -79,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");