can add multiple cbgs and check if callback is owned by another exec …

…before adding
ros2 · Jul 9, 2020 · b06b526 · b06b526
1 parent e97dcd4
commit b06b526
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 1 deletion.
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
@@ -84,6 +84,14 @@ class Executor
     rclcpp::CallbackGroup::SharedPtr group_ptr,
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
 
+
+  /// Add callback groups to an executor.
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_groups(
+    std::vector<rclcpp::CallbackGroup::SharedPtr> group_ptrs,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
   /// Remove a callback group from the executor.
   RCLCPP_PUBLIC
   virtual void

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -151,12 +151,25 @@ Executor::add_callback_group(
   }
 }
 
+void
+Executor::add_callback_groups(
+  std::vector<rclcpp::CallbackGroup::SharedPtr> group_ptrs,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
+  {
+    for_each(group_ptrs.begin(), group_ptrs.end(),
+    [node_ptr, notify, this](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+        add_callback_group(group_ptr, node_ptr, notify);
+    });
+  }
+
 void
 Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
 {
   for (auto & weak_group : node_ptr->get_callback_groups()) {
     auto group_ptr = weak_group.lock();
-    if (group_ptr != nullptr) {
+    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load()) {
       add_callback_group(group_ptr, node_ptr, notify);
     }
   }