diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 11bfc96..e174774 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -186,7 +186,8 @@ class RosActionNode : public BT::ActionNodeBase struct ActionClientInstance { ActionClientInstance(std::shared_ptr node, - const std::string& action_name); + const std::string& action_name, + const rcl_action_client_options_t& action_client_options); ActionClientPtr action_client; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -234,6 +235,7 @@ class RosActionNode : public BT::ActionNodeBase const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; std::string action_client_key_; + rcl_action_client_options_t action_client_options_; private: std::shared_future future_goal_handle_; @@ -253,12 +255,14 @@ class RosActionNode : public BT::ActionNodeBase template RosActionNode::ActionClientInstance::ActionClientInstance( - std::shared_ptr node, const std::string& action_name) + std::shared_ptr node, const std::string& action_name, + const rcl_action_client_options_t& action_client_options) { callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_executor.add_callback_group(callback_group, node->get_node_base_interface()); - action_client = rclcpp_action::create_client(node, action_name, callback_group); + action_client = rclcpp_action::create_client(node, action_name, callback_group, + action_client_options); } template @@ -269,6 +273,7 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, , node_(params.nh) , server_timeout_(params.server_timeout) , wait_for_server_timeout_(params.wait_for_server_timeout) + , action_client_options_(params.action_client_options) { // Three cases: // - we use the default action_name in RosNodeParams when port is empty @@ -320,8 +325,9 @@ inline bool RosActionNode::createClient(const std::string& action_name) auto it = registry.find(action_client_key_); if(it == registry.end() || it->second.expired()) { - client_instance_ = std::make_shared(node, action_name); - registry.insert_or_assign(action_client_key_, client_instance_); + client_instance_ = + std::make_shared(node, action_name, action_client_options_); + registry.insert_or_assign({ action_client_key_, client_instance_ }); } else { diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index dec8848..fdba3de 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -145,7 +145,8 @@ class RosServiceNode : public BT::ActionNodeBase struct ServiceClientInstance { ServiceClientInstance(std::shared_ptr node, - const std::string& service_name); + const std::string& service_name, + const rclcpp::QoS& qos); ServiceClientPtr service_client; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -190,6 +191,7 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; + rclcpp::QoS qos_; bool service_name_should_be_checked_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; @@ -212,7 +214,8 @@ class RosServiceNode : public BT::ActionNodeBase template inline RosServiceNode::ServiceClientInstance::ServiceClientInstance( - std::shared_ptr node, const std::string& service_name) + std::shared_ptr node, const std::string& service_name, + const rclcpp::QoS& qos) { callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -221,7 +224,7 @@ inline RosServiceNode::ServiceClientInstance::ServiceClientInstance( // For Jazzy and Later Support #if RCLCPP_VERSION_GTE(28, 0, 0) service_client = - node->create_client(service_name, rclcpp::ServicesQoS(), callback_group); + node->create_client(service_name, qos, callback_group); #else service_client = node->create_client(service_name, rmw_qos_profile_services_default, callback_group); @@ -236,6 +239,7 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, , node_(params.nh) , service_timeout_(params.server_timeout) , wait_for_service_timeout_(params.wait_for_server_timeout) + , qos_(params.service_qos) { // check port remapping auto portIt = config().input_ports.find("service_name"); @@ -282,8 +286,9 @@ inline bool RosServiceNode::createClient(const std::string& service_name) auto it = registry.find(client_key); if(it == registry.end() || it->second.expired()) { - srv_instance_ = std::make_shared(node, service_name); - registry.insert_or_assign(client_key, srv_instance_); + srv_instance_ = + std::make_shared(node, service_name, qos_); + registry.insert_or_assign({ client_key, srv_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(), service_name.c_str()); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index 8ccdc69..0a51a71 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -88,6 +88,7 @@ class RosTopicPubNode : public BT::ConditionNode std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; + rclcpp::QoS qos_; private: std::shared_ptr publisher_; @@ -103,7 +104,7 @@ template inline RosTopicPubNode::RosTopicPubNode(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_(params.topic_qos) { // check port remapping auto portIt = config().input_ports.find("topic_name"); @@ -158,7 +159,7 @@ inline bool RosTopicPubNode::createPublisher(const std::string& topic_name) throw RuntimeError("topic_name is empty"); } - publisher_ = node_->create_publisher(topic_name, 1); + publisher_ = node_->create_publisher(topic_name, qos_); prev_topic_name_ = topic_name; return true; } 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 975b1dc..05d2e98 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -51,7 +51,8 @@ class RosTopicSubNode : public BT::ConditionNode protected: struct SubscriberInstance { - SubscriberInstance(std::shared_ptr node, const std::string& topic_name); + SubscriberInstance(std::shared_ptr node, const std::string& topic_name, + const rclcpp::QoS& qos); std::shared_ptr subscriber; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -80,6 +81,7 @@ class RosTopicSubNode : public BT::ConditionNode std::shared_ptr sub_instance_; std::shared_ptr last_msg_; std::string topic_name_; + rclcpp::QoS qos_; boost::signals2::connection signal_connection_; std::string subscriber_key_; @@ -164,7 +166,8 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------------------------------------------------- template inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( - std::shared_ptr node, const std::string& topic_name) + std::shared_ptr node, const std::string& topic_name, + const rclcpp::QoS& qos) { // create a callback group for this particular instance callback_group = @@ -180,14 +183,14 @@ inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( last_msg = msg; broadcaster(msg); }; - subscriber = node->create_subscription(topic_name, 1, callback, option); + subscriber = node->create_subscription(topic_name, qos, callback, option); } template inline RosTopicSubNode::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_(params.topic_qos) { // check port remapping auto portIt = config().input_ports.find("topic_name"); @@ -261,8 +264,8 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) auto it = registry.find(subscriber_key_); if(it == registry.end() || it->second.expired()) { - sub_instance_ = std::make_shared(node, topic_name); - registry.insert_or_assign(subscriber_key_, sub_instance_); + sub_instance_ = std::make_shared(node, topic_name, qos_); + registry.insert_or_assign({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), topic_name.c_str()); diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index d9e1de7..bf69f5e 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -15,6 +15,8 @@ #pragma once #include +#include +#include #include #include #include @@ -34,6 +36,12 @@ struct RosNodeParams // - RosTopicSubNode: name of the topic to subscribe to std::string default_port_value; + // Default qos configuration for actions, services and topics + rcl_action_client_options_t action_client_options = + rcl_action_client_get_default_options(); + rclcpp::QoS service_qos = rclcpp::ServicesQoS(); + rclcpp::QoS topic_qos = rclcpp::SystemDefaultsQoS(); + // parameters used only by service client and action clients // timeout when sending a request