Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add QoS configuration to RosNodeParams #99

Open
wants to merge 3 commits into
base: humble
Choose a base branch
from
Open

Add QoS configuration to RosNodeParams #99

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6b92305
Add QoS to node params
rafal-gorecki Oct 21, 2024
70e81f5
Pre-commit
rafal-gorecki Oct 21, 2024
dc691bf
Add action options
rafal-gorecki Oct 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,8 @@ class RosActionNode : public BT::ActionNodeBase
struct ActionClientInstance
{
ActionClientInstance(std::shared_ptr<rclcpp::Node> 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;
Expand Down Expand Up @@ -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<typename GoalHandle::SharedPtr> future_goal_handle_;
Expand All @@ -241,12 +243,14 @@ class RosActionNode : public BT::ActionNodeBase

template <class T>
RosActionNode<T>::ActionClientInstance::ActionClientInstance(
std::shared_ptr<rclcpp::Node> node, const std::string& action_name)
std::shared_ptr<rclcpp::Node> 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<T>(node, action_name, callback_group);
action_client = rclcpp_action::create_client<T>(node, action_name, callback_group,
action_client_options);
}

template <class T>
Expand All @@ -257,6 +261,7 @@ inline RosActionNode<T>::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
Expand Down Expand Up @@ -308,7 +313,8 @@ inline bool RosActionNode<T>::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<ActionClientInstance>(node, action_name);
client_instance_ =
std::make_shared<ActionClientInstance>(node, action_name, action_client_options_);
registry.insert({ action_client_key_, client_instance_ });
}
else
Expand Down
14 changes: 9 additions & 5 deletions behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,8 @@ class RosServiceNode : public BT::ActionNodeBase
struct ServiceClientInstance
{
ServiceClientInstance(std::shared_ptr<rclcpp::Node> 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;
Expand Down Expand Up @@ -188,6 +189,7 @@ class RosServiceNode : public BT::ActionNodeBase

std::weak_ptr<rclcpp::Node> 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_;
Expand All @@ -210,14 +212,14 @@ class RosServiceNode : public BT::ActionNodeBase

template <class T>
inline RosServiceNode<T>::ServiceClientInstance::ServiceClientInstance(
std::shared_ptr<rclcpp::Node> node, const std::string& service_name)
std::shared_ptr<rclcpp::Node> 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<T>(service_name, rmw_qos_profile_services_default,
callback_group);
service_client = node->create_client<T>(service_name, qos_profile, callback_group);
}

template <class T>
Expand All @@ -228,6 +230,7 @@ inline RosServiceNode<T>::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");
Expand Down Expand Up @@ -274,7 +277,8 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
auto it = registry.find(client_key);
if(it == registry.end() || it->second.expired())
{
srv_instance_ = std::make_shared<ServiceClientInstance>(node, service_name);
srv_instance_ =
std::make_shared<ServiceClientInstance>(node, service_name, qos_profile_);
registry.insert({ client_key, srv_instance_ });

RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(),
Expand Down
7 changes: 4 additions & 3 deletions behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,7 @@ class RosTopicPubNode : public BT::ConditionNode
std::shared_ptr<rclcpp::Node> node_;
std::string prev_topic_name_;
bool topic_name_may_change_ = false;
rclcpp::QoS qos_;

private:
std::shared_ptr<Publisher> publisher_;
Expand All @@ -103,7 +104,7 @@ template <class T>
inline RosTopicPubNode<T>::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");
Expand Down Expand Up @@ -158,7 +159,7 @@ inline bool RosTopicPubNode<T>::createPublisher(const std::string& topic_name)
throw RuntimeError("topic_name is empty");
}

publisher_ = node_->create_publisher<T>(topic_name, 1);
publisher_ = node_->create_publisher<T>(topic_name, qos_);
prev_topic_name_ = topic_name;
return true;
}
Expand All @@ -175,7 +176,7 @@ inline NodeStatus RosTopicPubNode<T>::tick()
getInput("topic_name", topic_name);
if(prev_topic_name_ != topic_name)
{
createPublisher(topic_name);
createPublisher(topic_name, qos_);
}
}

Expand Down
13 changes: 8 additions & 5 deletions behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ 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);

std::shared_ptr<Subscriber> subscriber;
rclcpp::CallbackGroup::SharedPtr callback_group;
Expand Down Expand Up @@ -80,6 +81,7 @@ class RosTopicSubNode : public BT::ConditionNode
std::shared_ptr<SubscriberInstance> sub_instance_;
std::shared_ptr<TopicT> last_msg_;
std::string topic_name_;
rclcpp::QoS qos_;
boost::signals2::connection signal_connection_;
std::string subscriber_key_;

Expand Down Expand Up @@ -164,7 +166,8 @@ class RosTopicSubNode : public BT::ConditionNode
//----------------------------------------------------------------
template <class T>
inline RosTopicSubNode<T>::SubscriberInstance::SubscriberInstance(
std::shared_ptr<rclcpp::Node> node, const std::string& topic_name)
std::shared_ptr<rclcpp::Node> node, const std::string& topic_name,
const rclcpp::QoS& qos)
{
// create a callback group for this particular instance
callback_group =
Expand All @@ -180,14 +183,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, 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_(params.topic_qos)
{
// check port remapping
auto portIt = config().input_ports.find("topic_name");
Expand Down Expand Up @@ -261,7 +264,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_);
registry.insert({ subscriber_key_, sub_instance_ });

RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(),
Expand Down
9 changes: 9 additions & 0 deletions behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <rcl_action/action_client.h>
#include <rmw/qos_profiles.h>
#include <string>
#include <chrono>
#include <memory>
Expand All @@ -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
Expand Down