Merge upstream into humble

Author: GregoryLeMasurier

behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp

@@ -51,7 +51,7 @@ class RosTopicSubNode : public BT::ConditionNode
 protected:
   struct SubscriberInstance
   {
-    SubscriberInstance(std::shared_ptr<rclcpp::Node> node, const std::string& topic_name);
+    SubscriberInstance(std::shared_ptr<rclcpp::Node> node, const std::string& topic_name, const rclcpp::QoS& qos_profile);
 
     std::shared_ptr<Subscriber> subscriber;
     rclcpp::CallbackGroup::SharedPtr callback_group;
@@ -82,6 +82,7 @@ class RosTopicSubNode : public BT::ConditionNode
   std::string topic_name_;
   boost::signals2::connection signal_connection_;
   std::string subscriber_key_;
+  rclcpp::QoS qos_profile_;
 
   rclcpp::Logger logger()
   {
@@ -156,7 +157,8 @@ class RosTopicSubNode : public BT::ConditionNode
   }
 
 private:
-  bool createSubscriber(const std::string& topic_name);
+
+  bool createSubscriber(const std::string& topic_name, const rclcpp::QoS& qos_profile);
 };
 
 //----------------------------------------------------------------
@@ -180,14 +182,14 @@ inline RosTopicSubNode<T>::SubscriberInstance::SubscriberInstance(
     last_msg = msg;
     broadcaster(msg);
   };
-  subscriber = node->create_subscription<T>(topic_name, 1, callback, option);
+  subscriber = node->create_subscription<T>(topic_name, qos_profile, callback, option);
 }
 
 template <class T>
 inline RosTopicSubNode<T>::RosTopicSubNode(const std::string& instance_name,
                                            const NodeConfig& conf,
                                            const RosNodeParams& params)
-  : BT::ConditionNode(instance_name, conf), node_(params.nh)
+  : BT::ConditionNode(instance_name, conf), node_(params.nh), qos_profile_(params.qos_profile)
 {
   // check port remapping
   auto portIt = config().input_ports.find("topic_name");
@@ -204,15 +206,15 @@ inline RosTopicSubNode<T>::RosTopicSubNode(const std::string& instance_name,
       }
       else
       {
-        createSubscriber(params.default_port_value);
+        createSubscriber(params.default_port_value, params.qos_profile);
       }
     }
     else if(!isBlackboardPointer(bb_topic_name))
     {
       // If the content of the port "topic_name" is not
       // a pointer to the blackboard, but a static string, we can
       // create the client in the constructor.
-      createSubscriber(bb_topic_name);
+      createSubscriber(bb_topic_name, params.qos_profile);
     }
     else
     {
@@ -229,13 +231,13 @@ inline RosTopicSubNode<T>::RosTopicSubNode(const std::string& instance_name,
     }
     else
     {
-      createSubscriber(params.default_port_value);
+      createSubscriber(params.default_port_value, params.qos_profile);
     }
   }
 }
 
 template <class T>
-inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name)
+inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name, const rclcpp::QoS& qos_profile)
 {
   if(topic_name.empty())
   {
@@ -261,7 +263,7 @@ inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name)
   auto it = registry.find(subscriber_key_);
   if(it == registry.end() || it->second.expired())
   {
-    sub_instance_ = std::make_shared<SubscriberInstance>(node, topic_name);
+    sub_instance_ = std::make_shared<SubscriberInstance>(node, topic_name, qos_profile);
     registry.insert({ subscriber_key_, sub_instance_ });
 
     RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(),
@@ -283,6 +285,7 @@ inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name)
       [this](const std::shared_ptr<T> msg) { last_msg_ = msg; });
 
   topic_name_ = topic_name;
+
   return true;
 }
 
@@ -303,7 +306,9 @@ inline NodeStatus RosTopicSubNode<T>::tick()
 
   if(!sub_instance_)
   {
-    createSubscriber(topic_name);
+    std::string topic_name;
+    getInput("topic_name", topic_name);
+    createSubscriber(topic_name, qos_profile_); 
   }
 
   auto CheckStatus = [](NodeStatus status) {

behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp

@@ -47,6 +47,9 @@ struct RosNodeParams
   RosNodeParams(std::shared_ptr<rclcpp::Node> node, const std::string& port_name)
     : nh(node), default_port_value(port_name)
   {}
+
+  // parameter only used by the subscriber
+  rclcpp::QoS qos_profile=1;
 };
 
 }  // namespace BT