diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 05c664d..e373b23 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -174,7 +174,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; @@ -222,6 +223,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_; @@ -241,12 +243,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); 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 @@ -257,6 +261,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 @@ -308,7 +313,8 @@ 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); + client_instance_ = + std::make_shared(node, action_name, action_client_options_); registry.insert({ 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 77958cd..69299a8 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -143,7 +143,8 @@ class RosServiceNode : public BT::ActionNodeBase struct ServiceClientInstance { ServiceClientInstance(std::shared_ptr node, - const std::string& service_name); + const std::string& service_name, + const rmw_qos_profile_t& qos_profile); ServiceClientPtr service_client; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -188,6 +189,7 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; + rmw_qos_profile_t qos_profile_; bool service_name_should_be_checked_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; @@ -210,14 +212,14 @@ 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 rmw_qos_profile_t& qos_profile) { callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_executor.add_callback_group(callback_group, node->get_node_base_interface()); - service_client = node->create_client(service_name, rmw_qos_profile_services_default, - callback_group); + service_client = node->create_client(service_name, qos_profile, callback_group); } template @@ -228,6 +230,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_profile_(params.service_qos_profile) { // check port remapping auto portIt = config().input_ports.find("service_name"); @@ -274,7 +277,8 @@ 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); + srv_instance_ = + std::make_shared(node, service_name, qos_profile_); registry.insert({ client_key, srv_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", 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..0dcf265 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; } @@ -175,7 +176,7 @@ inline NodeStatus RosTopicPubNode::tick() getInput("topic_name", topic_name); if(prev_topic_name_ != topic_name) { - createPublisher(topic_name); + createPublisher(topic_name, qos_); } } 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 211f41c..eeee844 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,7 +264,7 @@ 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); + sub_instance_ = std::make_shared(node, topic_name, qos_); registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", 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..4a18f05 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,13 @@ 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(); + rmw_qos_profile_t service_qos_profile = rmw_qos_profile_services_default; + rclcpp::QoS topic_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + // parameters used only by service client and action clients // timeout when sending a request