remove subscribers when tree is destroyed

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(