diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index f51ec00ab5..75409d1abb 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -57,7 +57,7 @@ class CallbackGroup RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) RCLCPP_PUBLIC - explicit CallbackGroup(CallbackGroupType group_type); + explicit CallbackGroup(CallbackGroupType group_type, bool allow_executor_to_add = true); template rclcpp::SubscriptionBase::SharedPtr diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 8e8bfcb591..cdea1a7b8a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -141,7 +141,9 @@ class Node : public std::enable_shared_from_this /// Create and return a callback group. RCLCPP_PUBLIC rclcpp::CallbackGroup::SharedPtr - create_callback_group(rclcpp::CallbackGroupType group_type); + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool allow_executor_to_add = true); /// Return the list of callback groups in the node. RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 4386ee5e55..e8a3b248c3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -83,7 +83,9 @@ class NodeBase : public NodeBaseInterface RCLCPP_PUBLIC rclcpp::CallbackGroup::SharedPtr - create_callback_group(rclcpp::CallbackGroupType group_type) override; + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool allow_executor_to_add = true) override; RCLCPP_PUBLIC rclcpp::CallbackGroup::SharedPtr diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index bcb469c0bf..1d95ef7104 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -106,7 +106,9 @@ class NodeBaseInterface RCLCPP_PUBLIC virtual rclcpp::CallbackGroup::SharedPtr - create_callback_group(rclcpp::CallbackGroupType group_type) = 0; + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool allow_executor_to_add = true) = 0; /// Return the default callback group. RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index d68b1ca0f6..df3363143b 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -19,10 +19,10 @@ using rclcpp::CallbackGroup; using rclcpp::CallbackGroupType; -CallbackGroup::CallbackGroup(CallbackGroupType group_type) +CallbackGroup::CallbackGroup(CallbackGroupType group_type, bool allow_executor_to_add) : type_(group_type), associated_with_executor_(false), - can_be_taken_from_(true), allow_executor_to_add_(true) -{} + can_be_taken_from_(true) +{allow_executor_to_add_.store(allow_executor_to_add);} std::atomic_bool & diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 1d78ca4340..861f491cf2 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -234,7 +234,9 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt weak_nodes_.push_back(node_ptr); for (auto & weak_group : node_ptr->get_callback_groups()) { auto group_ptr = weak_group.lock(); - if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load()) { + if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() && + group_ptr->allow_executor_to_add()) + { add_callback_group(group_ptr, node_ptr, notify); } } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b69d7c2baf..ed2d915dfb 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -211,9 +211,9 @@ Node::get_logger() const } rclcpp::CallbackGroup::SharedPtr -Node::create_callback_group(rclcpp::CallbackGroupType group_type) +Node::create_callback_group(rclcpp::CallbackGroupType group_type, bool allow_executor_to_add) { - return node_base_->create_callback_group(group_type); + return node_base_->create_callback_group(group_type, allow_executor_to_add); } bool diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index d8c13bd826..66db08dfeb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -216,11 +216,11 @@ NodeBase::get_shared_rcl_node_handle() const } rclcpp::CallbackGroup::SharedPtr -NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type) +NodeBase::create_callback_group(rclcpp::CallbackGroupType group_type, bool allow_executor_to_add) { using rclcpp::CallbackGroup; using rclcpp::CallbackGroupType; - auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); + auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type, allow_executor_to_add)); callback_groups_.push_back(group); return group; } diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 889ac58a5e..c018cd173c 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -188,7 +188,7 @@ TEST_F(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_ex std::atomic_int timer_count {0}; auto timer_callback = [&executor, &timer_count]() { if (timer_count > 0) { - ASSERT_EQ(executor.get_callback_groups().size(), 2u); + ASSERT_EQ(executor.get_callback_groups().size(), 3u); executor.cancel(); } timer_count++; @@ -197,5 +197,50 @@ TEST_F(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_ex rclcpp::callback_group::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( 2s, timer_callback, cb_grp); + rclcpp::callback_group::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive, false); + auto timer2_callback = []() {}; + rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( + 2s, timer2_callback, cb_grp2); + rclcpp::callback_group::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive, true); + auto timer3_callback = []() {}; + rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( + 2s, timer3_callback, cb_grp3); executor.spin(); } + +/* + * Test adding unallowable callback group. + */ +TEST_F(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) +{ + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::callback_group::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + ASSERT_EQ(executor.get_callback_groups().size(), 1u); + + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + rclcpp::callback_group::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive, false); + options.callback_group = cb_grp2; + auto subscription = + node->create_subscription("topic_name", qos, callback, options); + executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + ASSERT_EQ(executor.get_callback_groups().size(), 2u); + + auto timer2_callback = []() {}; + rclcpp::callback_group::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( + rclcpp::callback_group::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( + 2s, timer2_callback, cb_grp3); + executor.add_node(node->get_node_base_interface()); + ASSERT_EQ(executor.get_callback_groups().size(), 3u); +}