Prevent invoking callbacks after a node unsubscribes to a topic (#381)

Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>

Author: iche033

src/Node.cc

@@ -400,7 +400,8 @@ bool Node::Publisher::Publish(const ProtoMsg &_msg)
     {
       std::unique_lock<std::mutex> queueLock(
           this->dataPtr->shared->dataPtr->pubThreadMutex);
-      this->dataPtr->shared->dataPtr->pubQueue.push(std::move(pubMsgDetails));
+      this->dataPtr->shared->dataPtr->pubQueue.push_back(
+          std::move(pubMsgDetails));
     }
 
     this->dataPtr->shared->dataPtr->signalNewPub.notify_one();
@@ -601,6 +602,15 @@ bool Node::Unsubscribe(const std::string &_topic)
     return false;
   }
 
+  // Remove handlers from shared pubQueue to avoid invoking callbacks after
+  // unsuscribing to the topic
+  if (!this->dataPtr->RemoveHandlersFromPubQueue(topic))
+  {
+    std::cerr << "Error removing subscription handlers from publish queue "
+              << "when unsubscribing from Topic [" << _topic << "]"
+              <<  std::endl;
+  }
+
   std::lock_guard<std::recursive_mutex> lk(this->dataPtr->shared->mutex);
 
   // Remove the subscribers for the given topic that belong to this node.
@@ -1030,3 +1040,43 @@ bool Node::SubscribeHelper(const std::string &_fullyQualifiedTopic)
 {
   return this->dataPtr->SubscribeHelper(_fullyQualifiedTopic);
 }
+
+//////////////////////////////////////////////////
+bool NodePrivate::RemoveHandlersFromPubQueue(const std::string &_topic)
+{
+  // Remove from pubQueue
+  std::unique_lock<std::mutex> queueLock(
+      this->shared->dataPtr->pubThreadMutex);
+  for (auto &msgDetails : this->shared->dataPtr->pubQueue)
+  {
+    // check if there is a pub queue with message details that has topic
+    // which the node unsubscribes to
+    if (msgDetails->info.Topic() != _topic)
+      continue;
+
+    // remove local handler if it is a handler for this node
+    for (auto handlerIt = msgDetails->localHandlers.begin();
+         handlerIt != msgDetails->localHandlers.end();)
+    {
+      if ((*handlerIt)->NodeUuid() == this->nUuid)
+      {
+        msgDetails->localHandlers.erase(handlerIt);
+      }
+      else
+        ++handlerIt;
+    }
+
+    // remove raw handler if it is a handler for this node
+    for (auto handlerIt = msgDetails->rawHandlers.begin();
+         handlerIt != msgDetails->rawHandlers.end();)
+    {
+      if ((*handlerIt)->NodeUuid() == this->nUuid)
+      {
+        msgDetails->rawHandlers.erase(handlerIt);
+      }
+      else
+        ++handlerIt;
+    }
+  }
+  return true;
+}

src/NodePrivate.hh

@@ -50,6 +50,12 @@ namespace gz
       /// \sa TopicUtils::FullyQualifiedName
       public: bool SubscribeHelper(const std::string &_fullyQualifiedTopic);
 
+      /// \brief Helper function to remove handlers from the shared publish
+      /// queue. This is called when the node unsubscribes to a topic
+      /// \param[in] _topic Topic that the node unsubcribed to.
+      /// \return True on success.
+      public: bool RemoveHandlersFromPubQueue(const std::string &_topic);
+
       /// \brief The list of topics subscribed by this node.
       public: std::unordered_set<std::string> topicsSubscribed;
 

src/NodeShared.cc

@@ -1888,7 +1888,7 @@ void NodeSharedPrivate::PublishThread()
 
       // Get the message
       msgDetails = std::move(this->pubQueue.front());
-      this->pubQueue.pop();
+      this->pubQueue.pop_front();
     }
 
     // Send the message to all the local handlers.

src/NodeSharedPrivate.hh

@@ -21,9 +21,9 @@
 #include <zmq.hpp>
 
 #include <atomic>
+#include <list>
 #include <map>
 #include <memory>
-#include <queue>
 #include <string>
 #include <vector>
 
@@ -175,9 +175,9 @@ namespace gz
       /// \brief Mutex to protect the pubThread and pubQueue.
       public: std::mutex pubThreadMutex;
 
-      /// \brief Queue onto which new messages are pushed. The pubThread
+      /// \brief List onto which new messages are pushed. The pubThread
       /// will pop off the messages and send them to local subscribers.
-      public: std::queue<std::unique_ptr<PublishMsgDetails>> pubQueue;
+      public: std::list<std::unique_ptr<PublishMsgDetails>> pubQueue;
 
       /// \brief used to signal when new work is available
       public: std::condition_variable signalNewPub;

test/integration/CMakeLists.txt

@@ -3,6 +3,7 @@ set(TEST_TYPE "INTEGRATION")
 set(tests
   authPubSub.cc
   scopedTopic.cc
+  callback_scope_TEST.cc
   statistics.cc
   twoProcsPubSub.cc
   twoProcsSrvCall.cc

test/integration/callback_scope_TEST.cc

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <gz/msgs/int32.pb.h>
+
+#include <gtest/gtest.h>
+
+#include <gz/transport/Node.hh>
+
+TEST(CallbackScope, CleanupCorrectly)
+{
+  // test cleanup for serialized msgs
+  gz::transport::Node node;
+  auto publisher = node.Advertise<gz::msgs::Int32>("/my_topic");
+  gz::msgs::Int32 msg;
+  {
+    auto msg2 = std::make_unique<gz::msgs::Int32>();
+
+    std::function<void(const gz::msgs::Int32&)> callback =
+      [&msg2](const gz::msgs::Int32 &) {
+
+        if (nullptr == msg2)
+        {
+          FAIL();
+        }
+        return;
+      };
+
+    node.Subscribe("/my_topic", callback);
+    publisher.Publish(msg);
+    publisher.Publish(msg);
+    node.Unsubscribe("/my_topic");
+
+    // Clear msg2
+    msg2.reset();
+  }
+
+  // test cleanup for raw msgs
+  gz::transport::Node nodeRaw;
+  auto publisherRaw = nodeRaw.Advertise<gz::msgs::Int32>("/my_topic_raw");
+  gz::msgs::Int32 msgRaw;
+  {
+    auto msg2Raw = std::make_unique<gz::msgs::Int32>();
+    std::function<void(const char *_msg, const size_t,
+        const gz::transport::MessageInfo &)> rawCallback =
+            [&msg2Raw](const char *, const size_t,
+            const gz::transport::MessageInfo &) {
+
+        if (nullptr == msg2Raw)
+        {
+          FAIL();
+        }
+        return;
+      };
+
+    nodeRaw.SubscribeRaw("/my_topic_raw", rawCallback);
+    // use Publish intead of PublishRaw so the msgs end up in the pub queue
+    publisherRaw.Publish(msgRaw);
+    publisherRaw.Publish(msgRaw);
+    nodeRaw.Unsubscribe("/my_topic_raw");
+
+    msg2Raw.reset();
+  }
+}