refactored RosTopicSubNode

Author: facontidavide

behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp

@@ -22,10 +22,12 @@
 #include "behaviortree_cpp/bt_factory.h"
 
 #include "behaviortree_ros2/ros_node_params.hpp"
+#include <boost/signals2.hpp>
 
 namespace BT
 {
 
+
 /**
  * @brief Abstract class to wrap a Topic subscriber.
  * Considering the example in the tutorial:
@@ -43,11 +45,44 @@ namespace BT
 template<class TopicT>
 class RosTopicSubNode : public BT::ConditionNode
 {
-
-public:
+ public:
   // Type definitions
   using Subscriber = typename rclcpp::Subscription<TopicT>;
 
+ protected: 
+  struct SubscriberInstance
+  {
+    std::shared_ptr<Subscriber> subscriber;
+    rclcpp::CallbackGroup::SharedPtr callback_group;
+    rclcpp::executors::SingleThreadedExecutor callback_group_executor;
+    boost::signals2::signal<void (const std::shared_ptr<TopicT>&)> broadcaster;
+  };
+
+  static std::mutex& registryMutex()
+  {
+    static std::mutex sub_mutex;
+    return sub_mutex;
+  }
+
+  // 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, SubscriberInstance> subscribers_registry;
+    return subscribers_registry[key];
+  }
+
+  std::shared_ptr<rclcpp::Node> node_;
+  SubscriberInstance* sub_instance_ = nullptr;
+  std::shared_ptr<TopicT> last_msg_;
+  std::string topic_name_;
+
+  rclcpp::Logger logger()
+  {
+    return node_->get_logger();
+  }
+
+ public:
+
   /** You are not supposed to instantiate this class directly, the factory will do it.
    * To register this class into the factory, use:
    *
@@ -87,32 +122,17 @@ class RosTopicSubNode : public BT::ConditionNode
 
   NodeStatus tick() override final;
 
-  /** topicCallback is the callback invoked when a message is received.
-   */
-  virtual void topicCallback(const std::shared_ptr<TopicT> msg);
-
   /** Callback invoked in the tick. You must return either SUCCESS of FAILURE
    *
-   * @param last_msg the latest message received since the last tick.
-   * it might be empty.
+   * @param last_msg the latest message received, since the last tick.
+   *                  Will be empty if no new message received.
+   * 
    * @return the new status of the Node, based on last_msg
    */
-  virtual BT::NodeStatus onTick(const typename TopicT::SharedPtr& last_msg) = 0;
-
-protected:
-
-  std::shared_ptr<rclcpp::Node> node_;
-  std::string prev_topic_name_;
-  bool topic_name_may_change_ = false;
+  virtual NodeStatus onTick(const std::shared_ptr<TopicT>& last_msg) = 0;
 
 private:
 
-  std::shared_ptr<Subscriber> subscriber_;
-  typename TopicT::SharedPtr last_msg_;
-  bool is_message_received_;
-  rclcpp::CallbackGroup::SharedPtr callback_group_;
-  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
-
   bool createSubscriber(const std::string& topic_name);
 };
 
@@ -151,7 +171,7 @@ template<class T> inline
       createSubscriber(bb_topic_name);
     }
     else {
-      topic_name_may_change_ = true;
+      // do nothing
       // createSubscriber will be invoked in the first tick().
     }
   }
@@ -173,48 +193,74 @@ template<class T> inline
   {
     throw RuntimeError("topic_name is empty");
   }
-  
-  callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
-  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
-  rclcpp::SubscriptionOptions sub_option;
-  sub_option.callback_group = callback_group_;
-  auto callback = std::bind(&RosTopicSubNode::topicCallback, this, std::placeholders::_1);
-  subscriber_ = node_->create_subscription<T>(topic_name, 1, callback, sub_option);
-  prev_topic_name_ = topic_name;
+  if(sub_instance_)
+  {
+    throw RuntimeError("Can't call createSubscriber more than once");
+  }
+
+  // find SubscriberInstance in the registry
+  std::unique_lock lk(registryMutex());
+  const auto key = topic_name + "@" + node_->get_fully_qualified_name();
+  sub_instance_ = &(getRegisteredInstance(key));
+
+  // just created (subscriber is not initialized)
+  if(!sub_instance_->subscriber)
+  {
+    // create a callback group for this particular instance
+    sub_instance_->callback_group = 
+      node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    sub_instance_->callback_group_executor.add_callback_group(
+      sub_instance_->callback_group, node_->get_node_base_interface());
+
+    rclcpp::SubscriptionOptions sub_option;
+      sub_option.callback_group = sub_instance_->callback_group;
+
+    // The callback will broadcast to all the instances of RosTopicSubNode<T>
+    auto callback = [this](const std::shared_ptr<T> msg) {
+      sub_instance_->broadcaster(msg);
+    };
+    sub_instance_->subscriber = 
+      node_->create_subscription<T>(topic_name, 1, callback, sub_option);
+
+    RCLCPP_INFO(logger(), 
+      "Node [%s] created Subscriber to topic [%s]",
+      name().c_str(), topic_name.c_str() );
+  }
+
+  // add "this" as received of the broadcaster
+  sub_instance_->broadcaster.connect([this](const std::shared_ptr<T> msg)
+  {
+    last_msg_ = msg;
+  });
+    
+  topic_name_ = topic_name;
   return true;
 }
 
-template<class T> inline
-  void RosTopicSubNode<T>::topicCallback(const std::shared_ptr<T> msg)
-{
-  last_msg_ = msg;
-}
 
 template<class T> inline
   NodeStatus RosTopicSubNode<T>::tick()
 {
   // First, check if the subscriber_ is valid and that the name of the
   // topic_name in the port didn't change.
   // otherwise, create a new subscriber
-  if(!subscriber_ || (status() == NodeStatus::IDLE && topic_name_may_change_))
+  if(!sub_instance_)
   {
     std::string topic_name;
     getInput("topic_name", topic_name);
-    if(prev_topic_name_ != topic_name)
-    {
-      createSubscriber(topic_name);
-    }
+    createSubscriber(topic_name); 
   }
 
   auto CheckStatus = [](NodeStatus status)
   {
     if( !isStatusCompleted(status) )
     {
-      throw std::logic_error("RosTopicSubNode: the callback must return either SUCCESS or FAILURE");
+      throw std::logic_error("RosTopicSubNode: the callback must return" 
+                             "either SUCCESS or FAILURE");
     }
     return status;
   };
-  callback_group_executor_.spin_some();
+  sub_instance_->callback_group_executor.spin_some();
   auto status = CheckStatus (onTick(last_msg_));
   last_msg_ = nullptr;
 

btcpp_ros2_samples/CMakeLists.txt

@@ -7,10 +7,12 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(ament_cmake REQUIRED)
 find_package(behaviortree_ros2 REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(btcpp_ros2_interfaces REQUIRED)
 
 set(THIS_PACKAGE_DEPS 
     behaviortree_ros2
+    std_msgs
     btcpp_ros2_interfaces )
 
 ######################################################
@@ -38,6 +40,11 @@ ament_target_dependencies(sleep_client_dyn ${THIS_PACKAGE_DEPS})
 add_executable(sleep_server src/sleep_server.cpp)
 ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) 
 
+######################################################
+# Build subscriber_test
+add_executable(subscriber_test src/subscriber_test.cpp)
+ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) 
+
 ######################################################
 # INSTALL
 
@@ -46,6 +53,7 @@ install(TARGETS
   sleep_client_dyn
   sleep_server
   sleep_plugin
+  subscriber_test
   DESTINATION lib/${PROJECT_NAME}
   )
 

btcpp_ros2_samples/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>behaviortree_ros2</depend>
+  <depend>std_msgs</depend>
   <depend>btcpp_ros2_interfaces</depend>
 
 <export>

btcpp_ros2_samples/src/subscriber_test.cpp

@@ -0,0 +1,63 @@
+#include "behaviortree_ros2/bt_topic_sub_node.hpp"
+#include <std_msgs/msg/string.hpp>
+
+using namespace BT;
+
+class ReceiveString: public RosTopicSubNode<std_msgs::msg::String>
+{
+public:
+  ReceiveString(const std::string& name,
+                const NodeConfig& conf,
+                const RosNodeParams& params)
+    : RosTopicSubNode<std_msgs::msg::String>(name, conf, params)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+  NodeStatus onTick(const std::shared_ptr<std_msgs::msg::String>& last_msg) override
+  {
+    if(last_msg) // empty if no new message received, since the last tick
+    {
+      RCLCPP_INFO(logger(), "New message: %s",  last_msg->data.c_str());   
+    }
+    return NodeStatus::SUCCESS;
+  }
+};
+
+  // Simple tree, used to execute once each action.
+  static const char* xml_text = R"(
+  <root BTCPP_format="4">
+    <BehaviorTree>
+      <Sequence>
+        <ReceiveString/>
+        <ReceiveString/>
+        <ReceiveString/>
+      </Sequence>
+    </BehaviorTree>
+  </root>
+ )";
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  auto nh = std::make_shared<rclcpp::Node>("subscriber_test");
+
+  BehaviorTreeFactory factory;
+
+  RosNodeParams params;
+  params.nh = nh;
+  params.default_port_value = "btcpp_string";
+  factory.registerNodeType<ReceiveString>("ReceiveString", params);
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  while(rclcpp::ok())
+  {
+    tree.tickWhileRunning();
+  }
+
+  return 0;
+}