From 21e6a85fedec79a7821c79aaa3bf19f7f4b42649 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 23 Jul 2021 14:00:57 +0000 Subject: [PATCH] Remove use of get_callback_groups(). This is not thread-safe, and we can do exactly the same thing by getting the default callback group and storing our custom callback group within the class. Signed-off-by: Chris Lalancette --- .../include/examples_rclcpp_cbg_executor/pong_node.hpp | 2 ++ .../src/examples_rclcpp_cbg_executor/pong_node.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/pong_node.hpp b/rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/pong_node.hpp index f47fec71..0469e9fe 100644 --- a/rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/pong_node.hpp +++ b/rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/pong_node.hpp @@ -37,6 +37,8 @@ class PongNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr get_low_prio_callback_group(); private: + rclcpp::CallbackGroup::SharedPtr low_prio_callback_group_; + rclcpp::Subscription::SharedPtr high_ping_subscription_; rclcpp::Publisher::SharedPtr high_pong_publisher_; void high_ping_received(const std_msgs::msg::Int32::SharedPtr msg); diff --git a/rclcpp/executors/cbg_executor/src/examples_rclcpp_cbg_executor/pong_node.cpp b/rclcpp/executors/cbg_executor/src/examples_rclcpp_cbg_executor/pong_node.cpp index d1fb620a..9984317c 100644 --- a/rclcpp/executors/cbg_executor/src/examples_rclcpp_cbg_executor/pong_node.cpp +++ b/rclcpp/executors/cbg_executor/src/examples_rclcpp_cbg_executor/pong_node.cpp @@ -36,13 +36,13 @@ PongNode::PongNode() "high_ping", rclcpp::SensorDataQoS(), std::bind(&PongNode::high_ping_received, this, _1)); - auto second_cb_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - assert(second_cb_group == get_low_prio_callback_group()); + low_prio_callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); declare_parameter("low_busyloop", 0.01); low_pong_publisher_ = this->create_publisher("low_pong", rclcpp::SensorDataQoS()); rclcpp::SubscriptionOptionsWithAllocator> options; - options.callback_group = second_cb_group; + options.callback_group = low_prio_callback_group_; low_ping_subscription_ = this->create_subscription( "low_ping", rclcpp::SensorDataQoS(), std::bind(&PongNode::low_ping_received, this, _1), options); @@ -50,12 +50,12 @@ PongNode::PongNode() rclcpp::CallbackGroup::SharedPtr PongNode::get_high_prio_callback_group() { - return get_callback_groups()[0].lock(); // ... the default callback group. + return get_node_base_interface()->get_default_callback_group(); // the default callback group. } rclcpp::CallbackGroup::SharedPtr PongNode::get_low_prio_callback_group() { - return get_callback_groups()[1].lock(); // ... the second callback group created in the ctor. + return low_prio_callback_group_; // the second callback group created in the ctor. } void PongNode::high_ping_received(const std_msgs::msg::Int32::SharedPtr msg)