From d4da1cfb3069e813409124376be5902ff1054dfc Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Mon, 15 Jan 2024 11:48:00 +0100
Subject: [PATCH] remove subscribers when tree is destroyed

---
 .../behaviortree_ros2/bt_topic_sub_node.hpp   | 39 +++++++++++++++----
 1 file changed, 31 insertions(+), 8 deletions(-)

diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
index 4a97079..311594b 100644
--- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
+++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
@@ -86,17 +86,18 @@ class RosTopicSubNode : public BT::ConditionNode
   }
 
   // contains the fully-qualified name of the node and the name of the topic
-  static SubscriberInstance& getRegisteredInstance(const std::string& key)
+  static std::unordered_map<std::string, std::shared_ptr<SubscriberInstance>>& getRegistry()
   {
-    static std::unordered_map<std::string, SubscriberInstance> subscribers_registry;
-    return subscribers_registry[key];
+    static std::unordered_map<std::string, std::shared_ptr<SubscriberInstance>> subscribers_registry;
+    return subscribers_registry;
   }
 
   std::shared_ptr<rclcpp::Node> node_;
-  SubscriberInstance* sub_instance_ = nullptr;
+  std::shared_ptr<SubscriberInstance> sub_instance_ = nullptr;
   std::shared_ptr<TopicT> last_msg_;
   std::string topic_name_;
   boost::signals2::connection signal_connection_;
+  std::string subscriber_key_;
 
   rclcpp::Logger logger()
   {
@@ -119,6 +120,22 @@ class RosTopicSubNode : public BT::ConditionNode
   virtual ~RosTopicSubNode() 
   {
     signal_connection_.disconnect();
+    // remove the subscribers from the static registry when the ALL the
+    // instances of RosTopicSubNode are destroyed (i.e., the tree is destroyed)
+    if(sub_instance_)
+    {
+      sub_instance_.reset();
+      std::unique_lock lk(registryMutex());
+      auto& registry = getRegistry();
+      auto it = registry.find(subscriber_key_);
+      // when the reference count is 1, means that the only instance is owned by the
+      // registry itself. Delete it
+      if(it != registry.end() && it->second.use_count() <= 1)
+      {
+        registry.erase(it);
+        RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() );
+      }
+    }
   }
 
   /**
@@ -225,18 +242,24 @@ template<class T> inline
 
   // find SubscriberInstance in the registry
   std::unique_lock lk(registryMutex());
-  const auto key = topic_name + "@" + node_->get_fully_qualified_name();
-  sub_instance_ = &(getRegisteredInstance(key));
+  subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name;
 
-  // just created (subscriber is not initialized)
-  if(!sub_instance_->subscriber)
+  auto& registry = getRegistry();
+  auto it = registry.find(subscriber_key_);
+  if(it == registry.end())
   {
+    it = registry.insert( {subscriber_key_, std::make_shared<SubscriberInstance>() }).first;
+    sub_instance_ = it->second;
     sub_instance_->init(node_, topic_name);
 
     RCLCPP_INFO(logger(), 
       "Node [%s] created Subscriber to topic [%s]",
       name().c_str(), topic_name.c_str() );
   }
+  else {
+    sub_instance_ = it->second;
+  }
+
 
   // add "this" as received of the broadcaster
   signal_connection_ = sub_instance_->broadcaster.connect(