exposed allowable state at the node level and added unit tests

ros2 · Jul 28, 2020 · 5f408bc · 5f408bc
1 parent f8bf48b
commit 5f408bc
Show file tree

Hide file tree

Showing 9 changed files with 66 additions and 13 deletions.
diff --git 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<typename Function>
   rclcpp::SubscriptionBase::SharedPtr

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
@@ -141,7 +141,9 @@ class Node : public std::enable_shared_from_this<Node>
   /// 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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<rclcpp::Node>("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<test_msgs::msg::Empty>("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);
+}