fix errors in test_entities_collector.cpp

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

rclcpp/CMakeLists.txt

@@ -53,9 +53,10 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
+  src/rclcpp/executors/detail/entities_collector.cpp
+  src/rclcpp/executors/detail/static_executor_entities_collector.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
-  src/rclcpp/executors/static_executor_entities_collector.cpp
   src/rclcpp/executors/static_single_threaded_executor.cpp
   src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/future_return_code.cpp

rclcpp/include/rclcpp/executors/detail/entities_collector.hpp

@@ -0,0 +1,199 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXECUTORS__DETAIL__ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__DETAIL__ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+namespace detail
+{
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+class EntitiesCollector
+  : public rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(EntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EntitiesCollector() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  virtual ~EntitiesCollector();
+
+  /// Take the data so that it can be consumed with `execute`.
+  /**
+   * For `EntitiesCollector`, this always return `nullptr`.
+   * \sa rclcpp::Waitable::take_data()
+   */
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override;
+
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   * \return boolean whether the node from the callback group is new
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group_from_map
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group_from_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /**
+   * \see rclcpp::Executor::add_node()
+   * \throw std::runtime_error if node was already added
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /**
+   * \see rclcpp::Executor::remove_node()
+   * \throw std::runtime_error if no guard condition is associated with node.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_manually_added_callback_groups()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
+protected:
+  virtual
+  void
+  callback_group_added_impl(
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  virtual
+  void
+  node_added_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) = 0;
+
+  virtual
+  void
+  callback_group_removed_impl(
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  virtual
+  void
+  node_removed_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) = 0;
+
+  /// Return true if the node belongs to the collector
+  /**
+   * \param[in] node_ptr a node base interface shared pointer
+   * \param[in] weak_groups_to_nodes map to nodes to lookup
+   * \return boolean whether a node belongs the collector
+   */
+  bool
+  has_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
+
+  /// Add all callback groups that can be automatically added by any executor
+  /// and is not already associated with an executor from nodes
+  /// that are associated with executor
+  /**
+   * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
+   */
+  void
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
+
+  /// List of weak nodes registered in the static executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+};
+
+}  // namespace detail
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__DETAIL__ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/detail/static_executor_entities_collector.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
-#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#ifndef RCLCPP__EXECUTORS__DETAIL__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__DETAIL__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
 
 #include <chrono>
 #include <list>
@@ -24,6 +24,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/executors/detail/entities_collector.hpp"
 #include "rclcpp/experimental/executable_list.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategy.hpp"
@@ -34,12 +35,11 @@ namespace rclcpp
 {
 namespace executors
 {
-typedef std::map<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+namespace detail
+{
 
 class StaticExecutorEntitiesCollector final
-  : public rclcpp::Waitable,
+  : public rclcpp::executors::detail::EntitiesCollector,
   public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
 {
 public:
@@ -79,15 +79,6 @@ public:
   void
   execute(std::shared_ptr<void> & data) override;
 
-  /// Take the data so that it can be consumed with `execute`.
-  /**
-   * For `StaticExecutorEntitiesCollector`, this always return `nullptr`.
-   * \sa rclcpp::Waitable::take_data()
-   */
-  RCLCPP_PUBLIC
-  std::shared_ptr<void>
-  take_data() override;
-
   /// Function to add_handles_to_wait_set and wait for work and
   /**
    * block until the wait set is ready or until the timeout has been exceeded.
@@ -109,85 +100,6 @@ public:
   size_t
   get_number_of_ready_guard_conditions() override;
 
-  /// Add a callback group to an executor.
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /// Add a callback group to an executor.
-  /**
-   * \see rclcpp::Executor::add_callback_group
-   * \return boolean whether the node from the callback group is new
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr);
-
-  /// Remove a callback group from the executor.
-  /**
-   * \see rclcpp::Executor::remove_callback_group_from_map
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_callback_group_from_map(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
-
-  /**
-   * \see rclcpp::Executor::add_node()
-   * \throw std::runtime_error if node was already added
-   */
-  RCLCPP_PUBLIC
-  bool
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /**
-   * \see rclcpp::Executor::remove_node()
-   * \throw std::runtime_error if no guard condition is associated with node.
-   */
-  RCLCPP_PUBLIC
-  bool
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups();
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \see rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups();
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes();
-
   /// Complete all available queued work without blocking.
   /**
    * This function checks if after the guard condition was triggered
@@ -283,6 +195,23 @@ public:
   rclcpp::Waitable::SharedPtr
   get_waitable(size_t i) {return exec_list_.waitable[i];}
 
+protected:
+  void
+  callback_group_added_impl(
+    rclcpp::CallbackGroup::SharedPtr group) override;
+
+  void
+  node_added_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) override;
+
+  void
+  callback_group_removed_impl(
+    rclcpp::CallbackGroup::SharedPtr group) override;
+
+  void
+  node_removed_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) override;
+
 private:
   /// Function to reallocate space for entities in the wait set.
   /**
@@ -297,46 +226,18 @@ private:
   void
   fill_memory_strategy();
 
-  /// Return true if the node belongs to the collector
-  /**
-   * \param[in] node_ptr a node base interface shared pointer
-   * \param[in] weak_groups_to_nodes map to nodes to lookup
-   * \return boolean whether a node belongs the collector
-   */
-  bool
-  has_node(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
-
-  /// Add all callback groups that can be automatically added by any executor
-  /// and is not already associated with an executor from nodes
-  /// that are associated with executor
-  /**
-   * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
-   */
-  void
-  add_callback_groups_from_nodes_associated_to_executor();
-
   void
   fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   /// Memory strategy: an interface for handling user-defined memory allocation strategies.
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
-  // maps callback groups to nodes.
-  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
-  // maps callback groups to nodes.
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
-
   typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
       const rclcpp::GuardCondition *,
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
   WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
 
-  /// List of weak nodes registered in the static executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
-
   // Mutex to protect vector of new nodes.
   std::mutex new_nodes_mutex_;
   std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
@@ -351,7 +252,8 @@ private:
   bool initialized_ = false;
 };
 
+}  // namespace detail
 }  // namespace executors
 }  // namespace rclcpp
 
-#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#endif  // RCLCPP__EXECUTORS__DETAIL__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -25,7 +25,7 @@
 #include "rmw/rmw.h"
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
+#include "rclcpp/executors/detail/static_executor_entities_collector.hpp"
 #include "rclcpp/experimental/executable_list.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -214,7 +214,7 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
 
-  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+  detail::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 }  // namespace executors

rclcpp/src/rclcpp/executors/detail/entities_collector.cpp

@@ -0,0 +1,260 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executors/detail/entities_collector.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
+
+using rclcpp::executors::detail::EntitiesCollector;
+
+EntitiesCollector::~EntitiesCollector()
+{
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_nodes_.clear();
+}
+
+std::shared_ptr<void>
+EntitiesCollector::take_data()
+{
+  return nullptr;
+}
+
+bool
+EntitiesCollector::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  bool is_new_node = false;
+  // If the node already has an executor
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
+  node_ptr->for_each_callback_group(
+    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      if (
+        !group_ptr->get_associated_with_executor_atomic().load() &&
+        group_ptr->automatically_add_to_executor_with_node())
+      {
+        is_new_node = (add_callback_group(
+          group_ptr,
+          node_ptr,
+          weak_groups_to_nodes_associated_with_executor_) ||
+        is_new_node);
+      }
+    });
+  weak_nodes_.push_back(node_ptr);
+  return is_new_node;
+}
+
+bool
+EntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  // If the callback_group already has an executor
+  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Callback group has already been added to an executor.");
+  }
+  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto insert_info = weak_groups_to_nodes.insert(
+    std::make_pair(weak_group_ptr, node_ptr));
+  bool was_inserted = insert_info.second;
+  if (!was_inserted) {
+    throw std::runtime_error("Callback group was already added to executor.");
+  } else {
+    callback_group_added_impl(group_ptr);
+  }
+
+  if (is_new_node) {
+    node_added_impl(node_ptr);
+    return true;
+  }
+  return false;
+}
+
+bool
+EntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+EntitiesCollector::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr)
+{
+  return this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+EntitiesCollector::remove_callback_group_from_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+  if (iter != weak_groups_to_nodes.end()) {
+    node_ptr = iter->second.lock();
+    if (node_ptr == nullptr) {
+      throw std::runtime_error("Node must not be deleted before its callback group(s).");
+    }
+    weak_groups_to_nodes.erase(iter);
+    callback_group_removed_impl(group_ptr);
+  } else {
+    throw std::runtime_error("Callback group needs to be associated with executor.");
+  }
+  // If the node was matched and removed, interrupt waiting.
+  if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
+  {
+    node_removed_impl(node_ptr);
+    return true;
+  }
+  return false;
+}
+
+bool
+EntitiesCollector::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    return false;
+  }
+  bool node_found = false;
+  auto node_it = weak_nodes_.begin();
+  while (node_it != weak_nodes_.end()) {
+    bool matched = (node_it->lock() == node_ptr);
+    if (matched) {
+      weak_nodes_.erase(node_it);
+      node_found = true;
+      break;
+    }
+    ++node_it;
+  }
+  if (!node_found) {
+    return false;
+  }
+  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
+  std::for_each(
+    weak_groups_to_nodes_associated_with_executor_.begin(),
+    weak_groups_to_nodes_associated_with_executor_.end(),
+    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
+      auto & weak_node_ptr = key_value_pair.second;
+      auto shared_node_ptr = weak_node_ptr.lock();
+      auto group_ptr = key_value_pair.first.lock();
+      if (shared_node_ptr == node_ptr) {
+        found_group_ptrs.push_back(group_ptr);
+      }
+    });
+  std::for_each(
+    found_group_ptrs.begin(), found_group_ptrs.end(), [this]
+      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+      this->remove_callback_group_from_map(
+        group_ptr,
+        weak_groups_to_nodes_associated_with_executor_);
+    });
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
+  return true;
+}
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+EntitiesCollector::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}
+
+void
+EntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
+{
+  for (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      node->for_each_callback_group(
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
+        {
+          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
+          !shared_group_ptr->get_associated_with_executor_atomic().load())
+          {
+            add_callback_group(
+              shared_group_ptr,
+              node,
+              weak_groups_to_nodes_associated_with_executor_);
+          }
+        });
+    }
+  }
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EntitiesCollector::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EntitiesCollector::get_manually_added_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EntitiesCollector::get_automatically_added_callback_groups_from_nodes()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}

rclcpp/src/rclcpp/executors/detail/static_executor_entities_collector.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/executors/static_executor_entities_collector.hpp"
+#include "rclcpp/executors/detail/static_executor_entities_collector.hpp"
 
 #include <algorithm>
 #include <memory>
@@ -22,10 +22,9 @@
 #include <vector>
 
 #include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 
-using rclcpp::executors::StaticExecutorEntitiesCollector;
+using rclcpp::executors::detail::StaticExecutorEntitiesCollector;
 
 StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 {
@@ -35,6 +34,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
     if (group) {
       std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
+      callback_group_removed_impl(group);
     }
   }
   for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
@@ -42,6 +42,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
     if (group) {
       std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
+      callback_group_removed_impl(group);
     }
   }
   // Disassociate all nodes
@@ -50,12 +51,12 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
     if (node) {
       std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
       has_executor.store(false);
+      node_removed_impl(node);
     }
   }
-  weak_groups_associated_with_executor_to_nodes_.clear();
-  weak_groups_to_nodes_associated_with_executor_.clear();
+
   exec_list_.clear();
-  weak_nodes_.clear();
+  new_nodes_.clear();
   weak_nodes_to_guard_conditions_.clear();
 }
 
@@ -89,12 +90,6 @@ StaticExecutorEntitiesCollector::fini()
   exec_list_.clear();
 }
 
-std::shared_ptr<void>
-StaticExecutorEntitiesCollector::take_data()
-{
-  return nullptr;
-}
-
 void
 StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
 {
@@ -171,6 +166,7 @@ StaticExecutorEntitiesCollector::fill_executable_list()
   // Add the executor's waitable to the executable list
   exec_list_.add_waitable(shared_from_this());
 }
+
 void
 StaticExecutorEntitiesCollector::fill_executable_list_from_map(
   const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
@@ -273,7 +269,7 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
   // Add waitable guard conditions (one for each registered node) into the wait set.
   for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto & gc = pair.second;
-    detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc);
+    rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc);
   }
 }
 
@@ -283,151 +279,34 @@ size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
   return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
 }
 
-bool
-StaticExecutorEntitiesCollector::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+void
+StaticExecutorEntitiesCollector::callback_group_added_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
 {
-  bool is_new_node = false;
-  // If the node already has an executor
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Node has already been added to an executor.");
-  }
-  node_ptr->for_each_callback_group(
-    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    {
-      if (
-        !group_ptr->get_associated_with_executor_atomic().load() &&
-        group_ptr->automatically_add_to_executor_with_node())
-      {
-        is_new_node = (add_callback_group(
-          group_ptr,
-          node_ptr,
-          weak_groups_to_nodes_associated_with_executor_) ||
-        is_new_node);
-      }
-    });
-  weak_nodes_.push_back(node_ptr);
-  return is_new_node;
+  (void)group;
 }
 
-bool
-StaticExecutorEntitiesCollector::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+void
+StaticExecutorEntitiesCollector::node_added_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
 {
-  // If the callback_group already has an executor
-  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
-  if (has_executor.exchange(true)) {
-    throw std::runtime_error("Callback group has already been added to an executor.");
-  }
-  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
-    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
-  auto insert_info = weak_groups_to_nodes.insert(
-    std::make_pair(weak_group_ptr, node_ptr));
-  bool was_inserted = insert_info.second;
-  if (!was_inserted) {
-    throw std::runtime_error("Callback group was already added to executor.");
-  }
-  if (is_new_node) {
-    std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-    new_nodes_.push_back(node_ptr);
-    return true;
-  }
-  return false;
+  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+  new_nodes_.push_back(node);
 }
 
-bool
-StaticExecutorEntitiesCollector::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+void
+StaticExecutorEntitiesCollector::callback_group_removed_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
 {
-  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
+  (void)group;
 }
 
-bool
-StaticExecutorEntitiesCollector::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr)
+void
+StaticExecutorEntitiesCollector::node_removed_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
 {
-  return this->remove_callback_group_from_map(
-    group_ptr,
-    weak_groups_associated_with_executor_to_nodes_);
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_callback_group_from_map(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
-{
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
-  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
-  if (iter != weak_groups_to_nodes.end()) {
-    node_ptr = iter->second.lock();
-    if (node_ptr == nullptr) {
-      throw std::runtime_error("Node must not be deleted before its callback group(s).");
-    }
-    weak_groups_to_nodes.erase(iter);
-  } else {
-    throw std::runtime_error("Callback group needs to be associated with executor.");
-  }
-  // If the node was matched and removed, interrupt waiting.
-  if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
-    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
-  {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
-    return true;
-  }
-  return false;
-}
-
-bool
-StaticExecutorEntitiesCollector::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  if (!node_ptr->get_associated_with_executor_atomic().load()) {
-    return false;
-  }
-  bool node_found = false;
-  auto node_it = weak_nodes_.begin();
-  while (node_it != weak_nodes_.end()) {
-    bool matched = (node_it->lock() == node_ptr);
-    if (matched) {
-      weak_nodes_.erase(node_it);
-      node_found = true;
-      break;
-    }
-    ++node_it;
-  }
-  if (!node_found) {
-    return false;
-  }
-  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
-  std::for_each(
-    weak_groups_to_nodes_associated_with_executor_.begin(),
-    weak_groups_to_nodes_associated_with_executor_.end(),
-    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
-      auto & weak_node_ptr = key_value_pair.second;
-      auto shared_node_ptr = weak_node_ptr.lock();
-      auto group_ptr = key_value_pair.first.lock();
-      if (shared_node_ptr == node_ptr) {
-        found_group_ptrs.push_back(group_ptr);
-      }
-    });
-  std::for_each(
-    found_group_ptrs.begin(), found_group_ptrs.end(), [this]
-      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
-      this->remove_callback_group_from_map(
-        group_ptr,
-        weak_groups_to_nodes_associated_with_executor_);
-    });
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  has_executor.store(false);
-  return true;
+  rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node);
+  weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
 }
 
 bool
@@ -451,74 +330,3 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
   // None of the guard conditions triggered belong to a registered node
   return false;
 }
-
-// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
-bool
-StaticExecutorEntitiesCollector::has_node(
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
-  weak_groups_to_nodes) const
-{
-  return std::find_if(
-    weak_groups_to_nodes.begin(),
-    weak_groups_to_nodes.end(),
-    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
-      auto other_ptr = other.second.lock();
-      return other_ptr == node_ptr;
-    }) != weak_groups_to_nodes.end();
-}
-
-void
-StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
-{
-  for (const auto & weak_node : weak_nodes_) {
-    auto node = weak_node.lock();
-    if (node) {
-      node->for_each_callback_group(
-        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
-        {
-          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
-          {
-            add_callback_group(
-              shared_group_ptr,
-              node,
-              weak_groups_to_nodes_associated_with_executor_);
-          }
-        });
-    }
-  }
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_all_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_manually_added_callback_groups()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
-    groups.push_back(group_node_ptr.first);
-  }
-  return groups;
-}

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -22,6 +22,7 @@
 #include "rcpputils/scope_exit.hpp"
 
 using rclcpp::executors::StaticSingleThreadedExecutor;
+using rclcpp::executors::detail::StaticExecutorEntitiesCollector;
 using rclcpp::experimental::ExecutableList;
 
 StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -367,8 +367,8 @@ BENCHMARK_F(
   PerformanceTestExecutorSimple,
   static_executor_entities_collector_execute)(benchmark::State & st)
 {
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
-    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+  auto entities_collector_ =
+    std::make_shared<rclcpp::executors::detail::StaticExecutorEntitiesCollector>();
   entities_collector_->add_node(node->get_node_base_interface());
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();

rclcpp/test/rclcpp/CMakeLists.txt

@@ -644,6 +644,14 @@ if(TARGET test_multi_threaded_executor)
   target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+if(TARGET test_entities_collector)
+  ament_target_dependencies(test_entities_collector
+    "rcl")
+  target_link_libraries(test_entities_collector ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
 if(TARGET test_static_executor_entities_collector)

rclcpp/test/rclcpp/executors/test_entities_collector.cpp

@@ -0,0 +1,206 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <stdexcept>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcpputils/scope_exit.hpp"
+
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+class DummyEntitiesCollector
+  : public rclcpp::executors::detail::EntitiesCollector
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(DummyEntitiesCollector)
+
+  DummyEntitiesCollector() = default;
+
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+  }
+
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override
+  {
+    (void)wait_set;
+    return false;
+  }
+
+  std::shared_ptr<void>
+  take_data() override
+  {
+    return nullptr;
+  }
+
+  void
+  execute(std::shared_ptr<void> & data) override
+  {
+    (void)data;
+  }
+
+protected:
+  void
+  callback_group_added_impl(
+    rclcpp::CallbackGroup::SharedPtr group) override
+  {
+    (void)group;
+  }
+
+  void
+  node_added_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) override
+  {
+    (void)node;
+  }
+
+  void
+  callback_group_removed_impl(
+    rclcpp::CallbackGroup::SharedPtr group) override
+  {
+    (void)group;
+  }
+
+  void
+  node_removed_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) override
+  {
+    (void)node;
+  }
+};
+
+}  // namespace
+
+class TestEntitiesCollector : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    entities_collector_ =
+      std::make_shared<DummyEntitiesCollector>();
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+  DummyEntitiesCollector::SharedPtr entities_collector_;
+};
+
+TEST_F(TestEntitiesCollector, add_remove_node) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  // Check adding second time
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_node(node1->get_node_base_interface()),
+    std::runtime_error("Node has already been added to an executor."));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+
+  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
+  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
+}
+
+TEST_F(TestEntitiesCollector, add_callback_group) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+}
+
+TEST_F(TestEntitiesCollector, add_callback_group_after_add_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group has already been added to an executor."));
+}
+
+TEST_F(TestEntitiesCollector, add_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestEntitiesCollector, remove_callback_group_after_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestEntitiesCollector, remove_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  entities_collector_->remove_callback_group(cb_group);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Callback group needs to be associated with executor."));
+}
+
+TEST_F(TestEntitiesCollector, remove_node_opposite_order) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -88,7 +88,7 @@ public:
   {
     rclcpp::init(0, nullptr);
     entities_collector_ =
-      std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+      std::make_shared<rclcpp::executors::detail::StaticExecutorEntitiesCollector>();
   }
 
   void TearDown()
@@ -96,7 +96,7 @@ public:
     rclcpp::shutdown();
   }
 
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+  rclcpp::executors::detail::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) {
@@ -107,28 +107,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) {
   EXPECT_EQ(0u, entities_collector_->get_number_of_waitables());
 }
 
-TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
-
-  // Check adding second time
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_node(node1->get_node_base_interface()),
-    std::runtime_error("Node has already been added to an executor."));
-
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
-
-  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
-  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
-  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
-
-  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
-  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
-  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
-}
-
 TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   entities_collector_->add_node(node->get_node_base_interface());
@@ -315,39 +293,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
 }
 
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_node(node->get_node_base_interface());
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
-    std::runtime_error("Callback group has already been added to an executor."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-  cb_group->get_associated_with_executor_atomic().exchange(false);
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
-    std::runtime_error("Callback group was already added to executor."));
-}
-
 TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) {
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   entities_collector_->add_node(node->get_node_base_interface());
@@ -552,46 +497,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group)
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
 }
 
-TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-
-  node.reset();
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->remove_callback_group(cb_group),
-    std::runtime_error("Node must not be deleted before its callback group(s)."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-
-  entities_collector_->remove_callback_group(cb_group);
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->remove_callback_group(cb_group),
-    std::runtime_error("Callback group needs to be associated with executor."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
-
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
-
-  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
-}
-
 TEST_F(
   TestStaticExecutorEntitiesCollector,
   add_callback_groups_from_nodes_associated_to_executor_add) {