make timeouts and sleeps more robust

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

rclcpp/CHANGELOG.rst

@@ -2,26 +2,6 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-20.0.0 (2023-04-13)
--------------------
-* applied tracepoints for ring_buffer (`#2091 <https://github.com/ros2/rclcpp/issues/2091>`_)
-* Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 <https://github.com/ros2/rclcpp/issues/2165>`_)
-* Add type_hash to cpp TopicEndpointInfo (`#2137 <https://github.com/ros2/rclcpp/issues/2137>`_)
-* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_)
-* Minor grammar fix (`#2149 <https://github.com/ros2/rclcpp/issues/2149>`_)
-* Fix unnecessary allocations in executor.cpp (`#2135 <https://github.com/ros2/rclcpp/issues/2135>`_)
-* add Logger::get_effective_level(). (`#2141 <https://github.com/ros2/rclcpp/issues/2141>`_)
-* Remove deprecated header (`#2139 <https://github.com/ros2/rclcpp/issues/2139>`_)
-* Implement matched event (`#2105 <https://github.com/ros2/rclcpp/issues/2105>`_)
-* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_)
-* Fixes to silence some clang warnings. (`#2127 <https://github.com/ros2/rclcpp/issues/2127>`_)
-* Documentation improvements on the executor (`#2125 <https://github.com/ros2/rclcpp/issues/2125>`_)
-* Avoid losing waitable handles while using MultiThreadedExecutor (`#2109 <https://github.com/ros2/rclcpp/issues/2109>`_)
-* Hook up the incompatible type event inside of rclcpp (`#2069 <https://github.com/ros2/rclcpp/issues/2069>`_)
-* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
-* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_)
-* Contributors: Barry Xu, Chris Lalancette, Christopher Wecht, Emerson Knapp, Michael Carroll, Tomoya Fujita, Yadu, mauropasse, methylDragon, ymski
-
 19.3.0 (2023-03-01)
 -------------------
 * Fix memory leak in tracetools::get_symbol() (`#2104 <https://github.com/ros2/rclcpp/issues/2104>`_)

rclcpp/CMakeLists.txt

@@ -49,25 +49,19 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
   src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
-  src/rclcpp/dynamic_typesupport/dynamic_message.cpp
-  src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp
-  src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp
-  src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp
-  src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions/exceptions.cpp
+  src/rclcpp/experimental/executors/events_executor/events_executor_entities_collector.cpp
+  src/rclcpp/experimental/executors/events_executor/events_executor.cpp
+  src/rclcpp/experimental/timers_manager.cpp
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
-  src/rclcpp/executors/executor_entities_collection.cpp
-  src/rclcpp/executors/executor_entities_collector.cpp
-  src/rclcpp/executors/executor_notify_waitable.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/experimental/executors/events_executor/events_executor.cpp
-  src/rclcpp/experimental/timers_manager.cpp
   src/rclcpp/future_return_code.cpp
   src/rclcpp/generic_publisher.cpp
   src/rclcpp/generic_subscription.cpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -93,54 +93,11 @@ public:
    *   determines whether a callback group is automatically added to an executor
    *   with the node with which it is associated.
    */
-  [[deprecated("Use CallbackGroup constructor with context function argument")]]
   RCLCPP_PUBLIC
   explicit CallbackGroup(
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
-  /// Constructor for CallbackGroup.
-  /**
-   * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
-   * and when creating one the type must be specified.
-   *
-   * Callbacks in Reentrant Callback Groups must be able to:
-   *   - run at the same time as themselves (reentrant)
-   *   - run at the same time as other callbacks in their group
-   *   - run at the same time as other callbacks in other groups
-   *
-   * Callbacks in Mutually Exclusive Callback Groups:
-   *   - will not be run multiple times simultaneously (non-reentrant)
-   *   - will not be run at the same time as other callbacks in their group
-   *   - but must run at the same time as callbacks in other groups
-   *
-   * Additionally, callback groups have a property which determines whether or
-   * not they are added to an executor with their associated node automatically.
-   * When creating a callback group the automatically_add_to_executor_with_node
-   * argument determines this behavior, and if true it will cause the newly
-   * created callback group to be added to an executor with the node when the
-   * Executor::add_node method is used.
-   * If false, this callback group will not be added automatically and would
-   * have to be added to an executor manually using the
-   * Executor::add_callback_group method.
-   *
-   * Whether the node was added to the executor before creating the callback
-   * group, or after, is irrelevant; the callback group will be automatically
-   * added to the executor in either case.
-   *
-   * \param[in] group_type The type of the callback group.
-   * \param[in] get_node_context Lambda to retrieve the node context when
-   *   checking that the creating node is valid and using the guard condition.
-   * \param[in] automatically_add_to_executor_with_node A boolean that
-   *   determines whether a callback group is automatically added to an executor
-   *   with the node with which it is associated.
-   */
-  RCLCPP_PUBLIC
-  explicit CallbackGroup(
-    CallbackGroupType group_type,
-    std::function<rclcpp::Context::SharedPtr(void)> get_node_context,
-    bool automatically_add_to_executor_with_node = true);
-
   /// Default destructor.
   RCLCPP_PUBLIC
   ~CallbackGroup();
@@ -180,42 +137,14 @@ public:
     return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
   }
 
-  /// Get the total number of entities in this callback group.
-  /**
-   * \return the number of entities in the callback group.
-   */
-  RCLCPP_PUBLIC
-  size_t size() const;
-
-  /// Return a reference to the 'can be taken' atomic boolean.
-  /**
-   * The resulting bool will be true in the case that no executor is currently
-   * using an executable entity from this group.
-   * The resulting bool will be false in the case that an executor is currently
-   * using an executable entity from this group, and the group policy doesn't
-   * allow a second take (eg mutual exclusion)
-   * \return a reference to the flag
-   */
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
 
-  /// Get the group type.
-  /**
-   * \return the group type
-   */
   RCLCPP_PUBLIC
   const CallbackGroupType &
   type() const;
 
-  /// Collect all of the entity pointers contained in this callback group.
-  /**
-   * \param[in] sub_func Function to execute for each subscription
-   * \param[in] service_func Function to execute for each service
-   * \param[in] client_func Function to execute for each client
-   * \param[in] timer_func Function to execute for each timer
-   * \param[in] waitable_fuinc Function to execute for each waitable
-   */
   RCLCPP_PUBLIC
   void collect_all_ptrs(
     std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
@@ -249,24 +178,11 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
-  /// Retrieve the guard condition used to signal changes to this callback group.
-  /**
-   * \param[in] context_ptr context to use when creating the guard condition
-   * \return guard condition if it is valid, otherwise nullptr.
-   */
-  [[deprecated("Use get_notify_guard_condition() without arguments")]]
+  /// Defer creating the notify guard condition and return it.
   RCLCPP_PUBLIC
   rclcpp::GuardCondition::SharedPtr
   get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
 
-  /// Retrieve the guard condition used to signal changes to this callback group.
-  /**
-   * \return guard condition if it is valid, otherwise nullptr.
-   */
-  RCLCPP_PUBLIC
-  rclcpp::GuardCondition::SharedPtr
-  get_notify_guard_condition();
-
   /// Trigger the notify guard condition.
   RCLCPP_PUBLIC
   void
@@ -318,8 +234,6 @@ protected:
   std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
   std::recursive_mutex notify_guard_condition_mutex_;
 
-  std::function<rclcpp::Context::SharedPtr(void)> get_context_;
-
 private:
   template<typename TypeT, typename Function>
   typename TypeT::SharedPtr _find_ptrs_if_impl(

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -1,70 +0,0 @@
-// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
-#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
-
-#include <rcl/allocator.h>
-#include <rcl/types.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace dynamic_typesupport
-{
-
-/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t
-/// STUBBED OUT
-class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
-{
-public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
-
-  RCLCPP_PUBLIC
-  virtual ~DynamicMessage();
-
-protected:
-  // NOTE(methylDragon):
-  // This is just here to extend the lifetime of the serialization support
-  // It isn't actually used by the builder since the builder should compose its own support
-  //
-  // ... Though ideally it should be the exact same support as the one stored in the
-  // DynamicSerializationSupport
-  DynamicSerializationSupport::SharedPtr serialization_support_;
-
-  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_;
-  bool is_loaned_;
-
-  // Used for returning the loaned value, and lifetime management
-  DynamicMessage::SharedPtr parent_data_;
-
-private:
-  RCLCPP_DISABLE_COPY(DynamicMessage)
-
-  RCLCPP_PUBLIC
-  DynamicMessage();
-};
-
-}  // namespace dynamic_typesupport
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp

@@ -1,64 +0,0 @@
-// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
-#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
-
-#include <rcl/allocator.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace dynamic_typesupport
-{
-
-/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t`
-/// STUBBED OUT
-class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
-{
-public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
-
-  RCLCPP_PUBLIC
-  virtual ~DynamicMessageType();
-
-protected:
-  // NOTE(methylDragon):
-  // This is just here to extend the lifetime of the serialization support
-  // It isn't actually used by the builder since the builder should compose its own support
-  //
-  // ... Though ideally it should be the exact same support as the one stored in the
-  // `DynamicSerializationSupport`
-  DynamicSerializationSupport::SharedPtr serialization_support_;
-
-  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_;
-
-private:
-  RCLCPP_DISABLE_COPY(DynamicMessageType)
-
-  RCLCPP_PUBLIC
-  DynamicMessageType();
-};
-
-}  // namespace dynamic_typesupport
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp

@@ -1,65 +0,0 @@
-// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
-#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
-
-#include <rcl/allocator.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace dynamic_typesupport
-{
-
-/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *`
-/// STUBBED OUT
-class DynamicMessageTypeBuilder : public std::enable_shared_from_this<DynamicMessageTypeBuilder>
-{
-public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder)
-
-  RCLCPP_PUBLIC
-  virtual ~DynamicMessageTypeBuilder();
-
-protected:
-  // NOTE(methylDragon):
-  // This is just here to extend the lifetime of the serialization support
-  // It isn't actually used by the builder since the builder should compose its own support
-  //
-  // ... Though ideally it should be the exact same support as the one stored in the
-  // `DynamicSerializationSupport`
-  DynamicSerializationSupport::SharedPtr serialization_support_;
-
-  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_;
-
-private:
-  RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder)
-
-  RCLCPP_PUBLIC
-  DynamicMessageTypeBuilder();
-};
-
-}  // namespace dynamic_typesupport
-}  // namespace rclcpp
-
-
-#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -1,67 +0,0 @@
-// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
-#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
-
-#include <rcl/allocator.h>
-
-#include <rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h>
-#include <rosidl_dynamic_typesupport/types.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-#include <rosidl_runtime_c/type_description/type_description__struct.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace dynamic_typesupport
-{
-
-/// Utility wrapper class for `rosidl_message_type_support_t` containing managed
-/// STUBBED OUT
-class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
-{
-public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
-
-  RCLCPP_PUBLIC
-  virtual ~DynamicMessageTypeSupport();
-
-protected:
-  DynamicSerializationSupport::SharedPtr serialization_support_;
-  DynamicMessageType::SharedPtr dynamic_message_type_;
-  DynamicMessage::SharedPtr dynamic_message_;
-
-  rosidl_message_type_support_t rosidl_message_type_support_;
-
-private:
-  RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
-
-  RCLCPP_PUBLIC
-  DynamicMessageTypeSupport();
-};
-
-}  // namespace dynamic_typesupport
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp

@@ -1,60 +0,0 @@
-// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
-#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
-
-#include <rcl/allocator.h>
-#include <rosidl_dynamic_typesupport/api/serialization_support.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace dynamic_typesupport
-{
-
-/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t
-class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
-{
-public:
-  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
-
-  RCLCPP_PUBLIC
-  explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator());
-
-  RCLCPP_PUBLIC
-  DynamicSerializationSupport(
-    const std::string & serialization_library_name,
-    rcl_allocator_t allocator = rcl_get_default_allocator());
-
-  RCLCPP_PUBLIC
-  virtual ~DynamicSerializationSupport();
-
-protected:
-  rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_;
-
-private:
-  RCLCPP_DISABLE_COPY(DynamicSerializationSupport)
-};
-
-}  // namespace dynamic_typesupport
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -19,7 +19,6 @@
 #include <cassert>
 #include <chrono>
 #include <cstdlib>
-#include <deque>
 #include <iostream>
 #include <list>
 #include <map>
@@ -30,27 +29,28 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
-#include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/executor_entities_collector.hpp"
 #include "rclcpp/future_return_code.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set.hpp"
 
 namespace rclcpp
 {
 
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
 // Forward declaration is used in convenience method signature.
 class Node;
-class ExecutorImplementation;
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -402,6 +402,17 @@ public:
   void
   cancel();
 
+  /// Support dynamic switching of the memory strategy.
+  /**
+   * Switching the memory strategy while the executor is spinning in another threading could have
+   * unintended consequences.
+   * \param[in] memory_strategy Shared pointer to the memory strategy to set.
+   * \throws std::runtime_error if memory_strategy is null
+   */
+  RCLCPP_PUBLIC
+  void
+  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
+
   /// Returns true if the executor is currently spinning.
   /**
    * This function can be called asynchronously from any thread.
@@ -486,11 +497,6 @@ protected:
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
-  /// Gather all of the waitable entities from associated nodes and callback groups.
-  RCLCPP_PUBLIC
-  void
-  collect_entities();
-
   /// Block until more work becomes avilable or timeout is reached.
   /**
    * Builds a set of waitable entities, which are passed to the middleware.
@@ -502,6 +508,62 @@ protected:
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Find node associated with a callback group
+  /**
+   * \param[in] weak_groups_to_nodes map of callback groups to nodes
+   * \param[in] group callback group to find assocatiated node
+   * \return Pointer to associated node if found, else nullptr
+   */
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_by_group(
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  /// Return true if the node has been added to this executor.
+  /**
+   * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \param[in] weak_groups_to_nodes map to nodes to lookup
+   * \return true if the node is associated with the executor, otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  has_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
+
+  /// Find the callback group associated with a timer
+  /**
+   * \param[in] timer Timer to find associated callback group
+   * \return Pointer to callback group node if found, else nullptr
+   */
+  RCLCPP_PUBLIC
+  rclcpp::CallbackGroup::SharedPtr
+  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  /// Add a callback group to an executor
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_group_to_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  remove_callback_group_from_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
+
   /// Check for executable in ready state and populate union structure.
   /**
    * \param[out] any_executable populated union structure of ready executable
@@ -512,6 +574,33 @@ protected:
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * This is the implementation of get_next_ready_executable that takes into
+   * account the current state of callback groups' association with nodes and
+   * executors.
+   *
+   * This checks in a particular order for available work:
+   *   * Timers
+   *   * Subscriptions
+   *   * Services
+   *   * Clients
+   *   * Waitable
+   *
+   * If the next executable is not associated with this executor/node pair,
+   * then this method will return false.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] weak_groups_to_nodes mapping of callback groups to nodes
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  get_next_ready_executable_from_map(
+    AnyExecutable & any_executable,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
   /// Wait for executable in ready state and populate union structure.
   /**
    * If an executable is ready, it will return immediately, otherwise
@@ -529,17 +618,39 @@ protected:
     AnyExecutable & any_executable,
     std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Add all callback groups that can be automatically added from associated nodes.
+  /**
+   * The executor, before collecting entities, verifies if any callback group from
+   * nodes associated with the executor, which is not already associated to an executor,
+   * can be automatically added to this executor.
+   * This takes care of any callback group that has been added to a node but not explicitly added
+   * to the executor.
+   * It is important to note that in order for the callback groups to be automatically added to an
+   * executor through this function, the node of the callback groups needs to have been added
+   * through the `add_node` method.
+   */
+  RCLCPP_PUBLIC
+  virtual void
+  add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
+
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
+  rclcpp::GuardCondition interrupt_guard_condition_;
 
-  /// Guard condition for signaling the rmw layer to wake up for system shutdown.
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+
+  // Mutex to protect the subsequent memory_strategy_.
   mutable std::mutex mutex_;
 
+  /// The memory strategy: an interface for handling user-defined memory allocation strategies.
+  memory_strategy::MemoryStrategy::SharedPtr
+  memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
+
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
 
@@ -549,39 +660,42 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  /// Waitable containing guard conditions controlling the executor flow.
-  /**
-   * This waitable contains the interrupt and shutdown guard condition, as well
-   * as the guard condition associated with each node and callback group.
-   * By default, if any change is detected in the monitored entities, the notify
-   * waitable will awake the executor and rebuild  the collections.
-   */
-  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
 
-  std::atomic_bool entities_need_rebuild_;
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
 
-  /// Collector used to associate executable entities from nodes and guard conditions
-  rclcpp::executors::ExecutorEntitiesCollector collector_;
+  /// maps nodes to guard conditions
+  WeakNodesToGuardConditionsMap
+  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// Waitset to be waited on.
-  rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// Hold the current state of the collection being waited on by the waitset
-  rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
-    mutex_);
+  /// maps callback groups associated to nodes
+  WeakCallbackGroupsToNodesMap
+  weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// Hold the current state of the notify waitable being waited on by the waitset
-  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
-  RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to nodes associated with executor
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
-  /// Hold the list of executables currently available to be executed.
-  std::deque<rclcpp::AnyExecutable> ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps all callback groups to nodes
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// nodes that are associated with the executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
+  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// shutdown callback handle registered to Context
   rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
-
-  /// Pointer to implementation
-  std::unique_ptr<ExecutorImplementation> impl_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

@@ -1,213 +0,0 @@
-// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTION_HPP_
-#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
-
-#include <deque>
-#include <functional>
-#include <unordered_map>
-#include <vector>
-
-#include <rclcpp/any_executable.hpp>
-#include <rclcpp/node_interfaces/node_base.hpp>
-#include <rclcpp/callback_group.hpp>
-#include <rclcpp/executors/executor_notify_waitable.hpp>
-#include <rclcpp/visibility_control.hpp>
-#include <rclcpp/wait_result.hpp>
-#include <rclcpp/wait_set.hpp>
-
-namespace rclcpp
-{
-namespace executors
-{
-
-/// Structure to represent a single entity's entry in a collection
-template<typename EntityValueType>
-struct CollectionEntry
-{
-  /// Weak pointer to entity type
-  using EntityWeakPtr = typename EntityValueType::WeakPtr;
-  /// Shared pointer to entity type
-  using EntitySharedPtr = typename EntityValueType::SharedPtr;
-
-  /// The entity
-  EntityWeakPtr entity;
-
-  /// If relevant, the entity's corresponding callback_group
-  rclcpp::CallbackGroup::WeakPtr callback_group;
-};
-
-/// Update a collection based on another collection
-/*
- * Iterates update_from and update_to to see which entities have been added/removed between
- * the two collections.
- *
- * For each new entry (in update_from, but not in update_to),
- *   add the entity and fire the on_added callback
- * For each removed entry (in update_to, but not in update_from),
- *   remove the entity and fire the on_removed callback.
- *
- *  \param[in] update_from The collection representing the next iteration's state
- *  \param[inout] update_to The collection representing the current iteration's state
- *  \param[in] on_added Callback fired when a new entity is detected
- *  \param[in] on_removed Callback fired when an entity is removed
- */
-template<typename CollectionType>
-void update_entities(
-  const CollectionType & update_from,
-  CollectionType & update_to,
-  std::function<void(const typename CollectionType::EntitySharedPtr &)> on_added,
-  std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
-)
-{
-  for (auto it = update_to.begin(); it != update_to.end(); ) {
-    if (update_from.count(it->first) == 0) {
-      auto entity = it->second.entity.lock();
-      if (entity) {
-        on_removed(entity);
-      }
-      it = update_to.erase(it);
-    } else {
-      ++it;
-    }
-  }
-  for (auto it = update_from.begin(); it != update_from.end(); ++it) {
-    if (update_to.count(it->first) == 0) {
-      auto entity = it->second.entity.lock();
-      if (entity) {
-        on_added(entity);
-      }
-      update_to.insert(*it);
-    }
-  }
-}
-
-/// A collection of entities, indexed by their corresponding handles
-template<typename EntityKeyType, typename EntityValueType>
-class EntityCollection
-  : public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
-{
-public:
-  /// Key type of the map
-  using Key = const EntityKeyType *;
-
-  /// Weak pointer to entity type
-  using EntityWeakPtr = typename EntityValueType::WeakPtr;
-
-  /// Shared pointer to entity type
-  using EntitySharedPtr = typename EntityValueType::SharedPtr;
-
-  /// Update this collection based on the contents of another collection
-  /**
-   * Update the internal state of this collection, firing callbacks when entities have been
-   * added or removed.
-   *
-   * \param[in] other Collection to compare to
-   * \param[in] on_added Callback for when entities have been added
-   * \param[in] on_removed Callback for when entities have been removed
-   */
-  void update(
-    const EntityCollection<EntityKeyType, EntityValueType> & other,
-    std::function<void(const EntitySharedPtr &)> on_added,
-    std::function<void(const EntitySharedPtr &)> on_removed)
-  {
-    update_entities(other, *this, on_added, on_removed);
-  }
-};
-
-/// Represent the total set of entities for a single executor
-/**
- * This allows the entities to be stored from ExecutorEntitiesCollector.
- * The structure also makes in convenient to re-evaluate when entities have been added or removed.
- */
-struct ExecutorEntitiesCollection
-{
-  /// Collection type for timer entities
-  using TimerCollection = EntityCollection<rcl_timer_t, rclcpp::TimerBase>;
-
-  /// Collection type for subscription entities
-  using SubscriptionCollection = EntityCollection<rcl_subscription_t, rclcpp::SubscriptionBase>;
-
-  /// Collection type for client entities
-  using ClientCollection = EntityCollection<rcl_client_t, rclcpp::ClientBase>;
-
-  /// Collection type for service entities
-  using ServiceCollection = EntityCollection<rcl_service_t, rclcpp::ServiceBase>;
-
-  /// Collection type for waitable entities
-  using WaitableCollection = EntityCollection<rclcpp::Waitable, rclcpp::Waitable>;
-
-  /// Collection type for guard condition entities
-  using GuardConditionCollection = EntityCollection<rcl_guard_condition_t, rclcpp::GuardCondition>;
-
-  /// Collection of timers currently in use by the executor.
-  TimerCollection timers;
-
-  /// Collection of subscriptions currently in use by the executor.
-  SubscriptionCollection subscriptions;
-
-  /// Collection of clients currently in use by the executor.
-  ClientCollection clients;
-
-  /// Collection of services currently in use by the executor.
-  ServiceCollection services;
-
-  /// Collection of guard conditions currently in use by the executor.
-  GuardConditionCollection guard_conditions;
-
-  /// Collection of waitables currently in use by the executor.
-  WaitableCollection waitables;
-
-  /// Check if the entities collection is empty
-  /**
-   * \return true if all member collections are empty, false otherwise
-  */
-  bool empty() const;
-
-  /// Clear the entities collection
-  void clear();
-};
-
-/// Build an entities collection from callback groups
-/**
- * Iterates a list of callback groups and adds entities from each valid group
- *
- * \param[in] callback_groups List of callback groups to check for entities
- * \param[inout] colletion Entities collection to populate with found entities
- */
-void
-build_entities_collection(
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
-  ExecutorEntitiesCollection & collection);
-
-/// Build a queue of executables ready to be executed
-/**
- * Iterates a list of entities and adds them to a queue if they are ready.
- *
- * \param[in] collection Collection of entities corresponding to the current wait set.
- * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection.
- * \param[inout] queue of executables to append new ready executables to
- * \return number of new ready executables
- */
-size_t
-ready_executables(
-  const ExecutorEntitiesCollection & collection,
-  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
-  std::deque<rclcpp::AnyExecutable> & executables
-);
-}  // namespace executors
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_

rclcpp/include/rclcpp/executors/executor_entities_collector.hpp

@@ -1,270 +0,0 @@
-// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTOR_HPP_
-#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
-
-#include <map>
-#include <memory>
-#include <mutex>
-#include <set>
-#include <vector>
-
-#include "rcpputils/thread_safety_annotations.hpp"
-
-#include <rclcpp/any_executable.hpp>
-#include <rclcpp/node_interfaces/node_base.hpp>
-#include <rclcpp/callback_group.hpp>
-#include <rclcpp/executors/executor_notify_waitable.hpp>
-#include <rclcpp/visibility_control.hpp>
-#include <rclcpp/wait_set.hpp>
-#include <rclcpp/wait_result.hpp>
-
-namespace rclcpp
-{
-namespace executors
-{
-
-/// Class to monitor a set of nodes and callback groups for changes in entity membership
-/**
- * This is to be used with an executor to track the membership of various nodes, groups,
- * and entities (timers, subscriptions, clients, services, etc) and report status to the
- * executor.
- *
- * In general, users will add either nodes or callback groups to an executor.
- * Each node may have callback groups that are automatically associated with executors,
- * or callback groups that must be manually associated with an executor.
- *
- * This object tracks both types of callback groups as well as nodes that have been
- * previously added to the executor.
- * When a new callback group is added/removed or new entities are added/removed, the
- * corresponding node or callback group will signal this to the executor so that the
- * entity collection may be rebuilt according to that executor's implementation.
- *
- */
-class ExecutorEntitiesCollector
-{
-public:
-  /// Constructor
-  /**
-   * \param[in] notify_waitable Waitable that is used to signal to the executor
-   *   when nodes or callback groups have been added or removed.
-   */
-  RCLCPP_PUBLIC
-  explicit ExecutorEntitiesCollector(
-    std::shared_ptr<ExecutorNotifyWaitable> notify_waitable);
-
-  /// Destructor
-  RCLCPP_PUBLIC
-  ~ExecutorEntitiesCollector();
-
-  /// Indicate if the entities collector has pending additions or removals.
-  /**
-   * \return true if there are pending additions or removals
-   */
-  bool has_pending() const;
-
-  /// Add a node to the entity collector
-  /**
-   * \param[in] node_ptr a shared pointer that points to a node base interface
-   * \throw std::runtime_error if the node is associated with an executor
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /// Remove a node from the entity collector
-  /**
-   * \param[in] node_ptr a shared pointer that points to a node base interface
-   * \throw std::runtime_error if the node is associated with an executor
-   * \throw std::runtime_error if the node is associated with this executor
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
-
-  /// Add a callback group to the entity collector
-  /**
-   * \param[in] group_ptr a shared pointer that points to a callback group
-   * \throw std::runtime_error if the callback_group is associated with an executor
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
-
-  /// Remove a callback group from the entity collector
-  /**
-   * \param[in] group_ptr a shared pointer that points to a callback group
-   * \throw std::runtime_error if the callback_group is not associated with an executor
-   * \throw std::runtime_error if the callback_group is not associated with this executor
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
-
-  /// Get all callback groups known to this entity collector
-  /**
-   * This will include manually added and automatically added (node associated) groups
-   * \return vector of all callback groups
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups() const;
-
-  /// Get manually-added callback groups known to this entity collector
-  /**
-   * This will include callback groups that have been added via add_callback_group
-   * \return vector of manually-added callback groups
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups() const;
-
-  /// Get automatically-added callback groups known to this entity collector
-  /**
-   * This will include callback groups that are associated with nodes added via add_node
-   * \return vector of automatically-added callback groups
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups() const;
-
-  /// Update the underlying collections
-  /**
-   * This will prune nodes and callback groups that are no longer valid as well
-   * as add new callback groups from any associated nodes.
-   */
-  RCLCPP_PUBLIC
-  void
-  update_collections();
-
-protected:
-  using NodeCollection = std::set<
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
-
-  using CallbackGroupCollection = std::set<
-    rclcpp::CallbackGroup::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
-
-  using WeakNodesToGuardConditionsMap = std::map<
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-    rclcpp::GuardCondition::WeakPtr,
-    std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
-
-  using WeakGroupsToGuardConditionsMap = std::map<
-    rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::GuardCondition::WeakPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
-
-  /// Implementation of removing a node from the collector.
-  /**
-   * This will disassociate the node from the collector and remove any
-   * automatically-added callback groups
-   *
-   * This takes and returns an iterator so it may be used as:
-   *
-   * it = remove_weak_node(it);
-   *
-   * \param[in] weak_node iterator to the weak node to be removed
-   * \return Valid updated iterator in the same collection
-   */
-  RCLCPP_PUBLIC
-  NodeCollection::iterator
-  remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Implementation of removing a callback group from the collector.
-  /**
-   * This will disassociate the callback group from the collector
-   *
-   * This takes and returns an iterator so it may be used as:
-   *
-   * it = remove_weak_callback_group(it);
-   *
-   * \param[in] weak_group_it iterator to the weak group to be removed
-   * \param[in] collection the collection to remove the group from
-   *   (manually or automatically added)
-   * \return Valid updated iterator in the same collection
-   */
-  RCLCPP_PUBLIC
-  CallbackGroupCollection::iterator
-  remove_weak_callback_group(
-    CallbackGroupCollection::iterator weak_group_it,
-    CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Implementation of adding a callback group
-  /**
-   * \param[in] group_ptr the group to add
-   * \param[in] collection the collection to add the group to
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group_to_collection(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    CallbackGroupCollection & collection)  RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Iterate over queued added/remove nodes and callback_groups
-  RCLCPP_PUBLIC
-  void
-  process_queues() RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Check a collection of nodes and add any new callback_groups that
-  /// are set to be automatically associated via the node.
-  RCLCPP_PUBLIC
-  void
-  add_automatically_associated_callback_groups(
-    const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// Check all nodes and group for expired weak pointers and remove them.
-  RCLCPP_PUBLIC
-  void
-  prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
-
-  /// mutex to protect collections and pending queues
-  mutable std::mutex mutex_;
-
-  /// Callback groups that were added via `add_callback_group`
-  CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// Callback groups that were added by their association with added nodes
-  CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// nodes that are associated with the executor
-  NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// Track guard conditions associated with added nodes
-  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// Track guard conditions associated with added callback groups
-  WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// nodes that have been added since the last update.
-  NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// nodes that have been removed since the last update.
-  NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// callback groups that have been added since the last update.
-  CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// callback groups that have been removed since the last update.
-  CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
-  /// Waitable to add guard conditions to
-  std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_;
-};
-}  // namespace executors
-}  // namespace rclcpp
-//
-#endif  // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -1,158 +0,0 @@
-// Copyright 2023 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__EXECUTOR_NOTIFY_WAITABLE_HPP_
-#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
-
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <set>
-
-#include "rclcpp/guard_condition.hpp"
-#include "rclcpp/waitable.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-
-/// Maintain a collection of guard conditions from associated nodes and callback groups
-/// to signal to the executor when associated entities have changed.
-class ExecutorNotifyWaitable : public rclcpp::Waitable
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
-
-  // Constructor
-  /**
-   * \param[in] on_execute_callback Callback to execute when one of the conditions
-   *   of this waitable has signaled the wait_set.
-   */
-  RCLCPP_PUBLIC
-  explicit ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback = {});
-
-  // Destructor
-  RCLCPP_PUBLIC
-  ~ExecutorNotifyWaitable() override = default;
-
-  RCLCPP_PUBLIC
-  ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other);
-
-
-  RCLCPP_PUBLIC
-  ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other);
-
-  /// Add conditions to the wait set
-  /**
-   * \param[inout] wait_set structure that conditions will be added to
-   */
-  RCLCPP_PUBLIC
-  void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
-
-  /// Check conditions against the wait set
-  /**
-   * \param[inout] wait_set structure that internal elements will be checked against.
-   * \return true if this waitable is ready to be executed, false otherwise.
-   */
-  RCLCPP_PUBLIC
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override;
-
-  /// Perform work associated with the waitable.
-  /**
-   * This will call the callback provided in the constructor.
-   * \param[in] data Data to be use for the execute, if available, else nullptr.
-   */
-  RCLCPP_PUBLIC
-  void
-  execute(std::shared_ptr<void> & data) override;
-
-  /// Retrieve data to be used in the next execute call.
-  /**
-   * \return If available, data to be used, otherwise nullptr
-   */
-  RCLCPP_PUBLIC
-  std::shared_ptr<void>
-  take_data() override;
-
-  /// Take the data from an entity ID so that it can be consumed with `execute`.
-  /**
-   * \param[in] id ID of the entity to take data from.
-   * \return If available, data to be used, otherwise nullptr
-   * \sa rclcpp::Waitable::take_data_by_entity_id
-   */
-  RCLCPP_PUBLIC
-  std::shared_ptr<void>
-  take_data_by_entity_id(size_t id) override;
-
-  /// Set a callback to be called whenever the waitable becomes ready.
-  /**
-   * \param[in] callback callback to set
-   * \sa rclcpp::Waitable::set_on_ready_callback
-   */
-  RCLCPP_PUBLIC
-  void
-  set_on_ready_callback(std::function<void(size_t, int)> callback) override;
-
-  /// Add a guard condition to be waited on.
-  /**
-   * \param[in] guard_condition The guard condition to add.
-   */
-  RCLCPP_PUBLIC
-  void
-  add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition);
-
-  /// Unset any callback registered via set_on_ready_callback.
-  /**
-   * \sa rclcpp::Waitable::clear_on_ready_callback
-   */
-  RCLCPP_PUBLIC
-  void
-  clear_on_ready_callback() override;
-
-  /// Remove a guard condition from being waited on.
-  /**
-   * \param[in] weak_guard_condition The guard condition to remove.
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition);
-
-  /// Get the number of ready guard_conditions
-  /**
-   * \return The number of guard_conditions associated with the Waitable.
-   */
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_ready_guard_conditions() override;
-
-private:
-  /// Callback to run when waitable executes
-  std::function<void(void)> execute_callback_;
-
-  std::mutex guard_condition_mutex_;
-
-  std::function<void(size_t)> on_ready_callback_;
-
-  /// The collection of guard conditions to be waited on.
-  std::set<rclcpp::GuardCondition::WeakPtr,
-    std::owner_less<rclcpp::GuardCondition::WeakPtr>> notify_guard_conditions_;
-};
-
-}  // namespace executors
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -0,0 +1,357 @@
+// 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__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+class StaticExecutorEntitiesCollector final
+  : public rclcpp::Waitable,
+  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  StaticExecutorEntitiesCollector() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~StaticExecutorEntitiesCollector();
+
+  /// Initialize StaticExecutorEntitiesCollector
+  /**
+   * \param p_wait_set A reference to the wait set to be used in the executor
+   * \param memory_strategy Shared pointer to the memory strategy to set.
+   * \throws std::runtime_error if memory strategy is null
+   */
+  RCLCPP_PUBLIC
+  void
+  init(
+    rcl_wait_set_t * p_wait_set,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
+
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  bool
+  is_init() {return initialized_;}
+
+  RCLCPP_PUBLIC
+  void
+  fini();
+
+  /// Execute the waitable.
+  RCLCPP_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.
+   * \throws std::runtime_error if wait set couldn't be cleared or filled.
+   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
+   */
+  RCLCPP_PUBLIC
+  void
+  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /**
+   * \throws std::runtime_error if it couldn't add guard condition to wait set
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_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
+   * (or a spurious wakeup happened) we are really ready to execute
+   * i.e. re-collect entities
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  /// Return number of timers
+  /**
+   * \return number of timers
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_timers() {return exec_list_.number_of_timers;}
+
+  /// Return number of subscriptions
+  /**
+   * \return number of subscriptions
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
+
+  /// Return number of services
+  /**
+   * \return number of services
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_services() {return exec_list_.number_of_services;}
+
+  /// Return number of clients
+  /**
+   * \return number of clients
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_clients() {return exec_list_.number_of_clients;}
+
+  /// Return number of waitables
+  /**
+   * \return number of waitables
+   */
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_waitables() {return exec_list_.number_of_waitables;}
+
+  /** Return a SubscritionBase Sharedptr by index.
+   * \param[in] i The index of the SubscritionBase
+   * \return a SubscritionBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size of the structrue.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(size_t i) {return exec_list_.subscription[i];}
+
+  /** Return a TimerBase Sharedptr by index.
+   * \param[in] i The index of the TimerBase
+   * \return a TimerBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::TimerBase::SharedPtr
+  get_timer(size_t i) {return exec_list_.timer[i];}
+
+  /** Return a ServiceBase Sharedptr by index.
+   * \param[in] i The index of the ServiceBase
+   * \return a ServiceBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(size_t i) {return exec_list_.service[i];}
+
+  /** Return a ClientBase Sharedptr by index
+   * \param[in] i The index of the ClientBase
+   * \return a ClientBase shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(size_t i) {return exec_list_.client[i];}
+
+  /** Return a Waitable Sharedptr by index
+   * \param[in] i The index of the Waitable
+   * \return a Waitable shared pointer
+   * \throws std::out_of_range if the argument is higher than the size.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(size_t i) {return exec_list_.waitable[i];}
+
+private:
+  /// Function to reallocate space for entities in the wait set.
+  /**
+   * \throws std::runtime_error if wait set couldn't be cleared or resized.
+   */
+  void
+  prepare_wait_set();
+
+  void
+  fill_executable_list();
+
+  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_;
+
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t * p_wait_set_ = nullptr;
+
+  /// Executable list: timers, subscribers, clients, services and waitables
+  rclcpp::experimental::ExecutableList exec_list_;
+
+  /// Bool to check if the entities collector has been initialized
+  bool initialized_ = false;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -15,13 +15,24 @@
 #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
 
-#include <atomic>
 #include <chrono>
+#include <cassert>
+#include <cstdlib>
 #include <memory>
+#include <vector>
+#include <string>
+
+#include "rmw/rmw.h"
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/executors/static_executor_entities_collector.hpp"
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
@@ -54,7 +65,7 @@ public:
   explicit StaticSingleThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destructor.
+  /// Default destrcutor.
   RCLCPP_PUBLIC
   virtual ~StaticSingleThreadedExecutor();
 
@@ -105,20 +116,92 @@ public:
   void
   spin_all(std::chrono::nanoseconds max_duration) override;
 
+  /// Add a callback group to an executor.
+  /**
+   * \sa rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  void
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Remove callback group from the executor
+  /**
+   * \sa rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    bool notify = true) override;
+
+  /// Add a node to the executor.
+  /**
+   * \sa rclcpp::Executor::add_node
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \sa rclcpp::StaticSingleThreadedExecutor::add_node
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Remove a node from the executor.
+  /**
+   * \sa rclcpp::Executor::remove_node
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  /**
+   * \sa rclcpp::Executor::remove_node
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups() override;
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \sa rclcpp::Executor::get_manually_added_callback_groups()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups() override;
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes() override;
+
 protected:
   /**
    * @brief Executes ready executables from wait set.
-   * @param collection entities to evaluate for ready executables.
-   * @param wait_result result to check for ready executables.
    * @param spin_once if true executes only the first ready executable.
    * @return true if any executable was ready.
    */
   RCLCPP_PUBLIC
   bool
-  execute_ready_executables(
-    const rclcpp::executors::ExecutorEntitiesCollection & collection,
-    rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
-    bool spin_once);
+  execute_ready_executables(bool spin_once = false);
 
   RCLCPP_PUBLIC
   void
@@ -130,6 +213,8 @@ protected:
 
 private:
   RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
+
+  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -24,7 +24,6 @@
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
 #include "rclcpp/macros.hpp"
-#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -95,10 +94,6 @@ public:
 
     buffer_ = std::move(buffer_impl);
 
-    TRACEPOINT(
-      rclcpp_buffer_to_ipb,
-      static_cast<const void *>(buffer_.get()),
-      static_cast<const void *>(this));
     if (!allocator) {
       message_allocator_ = std::make_shared<MessageAlloc>();
     } else {

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -25,7 +25,6 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -52,7 +51,6 @@ public:
     if (capacity == 0) {
       throw std::invalid_argument("capacity must be a positive, non-zero value");
     }
-    TRACEPOINT(rclcpp_construct_ring_buffer, static_cast<const void *>(this), capacity_);
   }
 
   virtual ~RingBufferImplementation() {}
@@ -69,12 +67,6 @@ public:
 
     write_index_ = next_(write_index_);
     ring_buffer_[write_index_] = std::move(request);
-    TRACEPOINT(
-      rclcpp_ring_buffer_enqueue,
-      static_cast<const void *>(this),
-      write_index_,
-      size_ + 1,
-      is_full_());
 
     if (is_full_()) {
       read_index_ = next_(read_index_);
@@ -98,11 +90,6 @@ public:
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
-    TRACEPOINT(
-      rclcpp_ring_buffer_dequeue,
-      static_cast<const void *>(this),
-      read_index_,
-      size_ - 1);
     read_index_ = next_(read_index_);
 
     size_--;
@@ -148,10 +135,7 @@ public:
     return is_full_();
   }
 
-  void clear()
-  {
-    TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
-  }
+  void clear() {}
 
 private:
   /// Get the next index value for the ring buffer

rclcpp/include/rclcpp/experimental/executors/events_executor/event_waitable.hpp

@@ -0,0 +1,70 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_
+
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class provides a wrapper around the waitable object, that is
+ * meant to be used with the EventsExecutor.
+ * The waitset related methods are stubbed out as they should not be called.
+ * This class is abstract as the execute method of rclcpp::Waitable is not implemented.
+ * Nodes who want to implement a custom EventWaitable, can derive from this class and override
+ * the execute method.
+ */
+class EventWaitable : public rclcpp::Waitable
+{
+public:
+  // Constructor
+  RCLCPP_PUBLIC
+  EventWaitable() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  virtual ~EventWaitable() = default;
+
+  // Stub API: not used by EventsExecutor
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) final
+  {
+    (void)wait_set;
+    throw std::runtime_error("EventWaitable can't be checked if it's ready");
+    return false;
+  }
+
+  // Stub API: not used by EventsExecutor
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) final
+  {
+    (void)wait_set;
+    throw std::runtime_error("EventWaitable can't be added to a wait_set");
+  }
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp

@@ -15,15 +15,15 @@
 #ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
 #define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
 
-#include <atomic>
 #include <chrono>
 #include <memory>
+#include <queue>
 #include <vector>
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/executor_entities_collector.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_entities_collector.hpp"
 #include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_notify_waitable.hpp"
 #include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
 #include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
 #include "rclcpp/experimental/timers_manager.hpp"
@@ -40,9 +40,7 @@ namespace executors
 /**
  * This executor uses an events queue and a timers manager to execute entities from its
  * associated nodes and callback groups.
- * ROS 2 entities allow to set callback functions that are invoked when the entity is triggered
- * or has work to do. The events-executor sets these callbacks such that they push an
- * event into its queue.
+ * The RMW listener APIs are used to collect new events.
  *
  * This executor tries to reduce as much as possible the amount of maintenance operations.
  * This allows to use customized `EventsQueue` classes to achieve different goals such
@@ -185,10 +183,6 @@ public:
     rclcpp::CallbackGroup::SharedPtr group_ptr,
     bool notify = true) override;
 
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_all_callback_groups()
-   */
   RCLCPP_PUBLIC
   std::vector<rclcpp::CallbackGroup::WeakPtr>
   get_all_callback_groups() override;
@@ -210,12 +204,10 @@ public:
   get_automatically_added_callback_groups_from_nodes() override;
 
 protected:
-  /// Internal implementation of spin_once
   RCLCPP_PUBLIC
   void
   spin_once_impl(std::chrono::nanoseconds timeout) override;
 
-  /// Internal implementation of spin_some
   RCLCPP_PUBLIC
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
@@ -223,59 +215,18 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 
-  /// Execute a provided executor event if its associated entities are available
+  // Execute a single event
+  RCLCPP_PUBLIC
   void
   execute_event(const ExecutorEvent & event);
 
-  /// Collect entities from callback groups and refresh the current collection with them
-  void
-  refresh_current_collection_from_callback_groups();
-
-  /// Refresh the current collection using the provided new_collection
-  void
-  refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection);
-
-  /// Create a listener callback function for the provided entity
-  std::function<void(size_t)>
-  create_entity_callback(void * entity_key, ExecutorEventType type);
-
-  /// Create a listener callback function for the provided waitable entity
-  std::function<void(size_t, int)>
-  create_waitable_callback(const rclcpp::Waitable * waitable_id);
-
-  /// Searches for the provided entity_id in the collection and returns the entity if valid
-  template<typename CollectionType>
-  typename CollectionType::EntitySharedPtr
-  retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
-  {
-    // Check if the entity_id is in the collection
-    auto it = collection.find(entity_id);
-    if (it == collection.end()) {
-      return nullptr;
-    }
-
-    // Check if the entity associated with the entity_id is valid
-    // and remove it from the collection if it isn't
-    auto entity = it->second.entity.lock();
-    if (!entity) {
-      collection.erase(it);
-    }
-
-    // Return the retrieved entity (this can be a nullptr if the entity was not valid)
-    return entity;
-  }
-
-  /// Queue where entities can push events
+  // Queue where entities can push events
   rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
 
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
-  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
+  EventsExecutorEntitiesCollector::SharedPtr entities_collector_;
+  EventsExecutorNotifyWaitable::SharedPtr executor_notifier_;
 
-  /// Flag used to reduce the number of unnecessary waitable events
-  std::atomic<bool> notify_waitable_event_pushed_ {false};
-
-  /// Timers manager used to track and/or execute associated timers
+  // Timers manager
   std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
 };
 

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_entities_collector.hpp

@@ -0,0 +1,348 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <map>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/timers_manager.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 experimental
+{
+namespace executors
+{
+// forward declaration of EventsExecutor to avoid circular dependency
+class EventsExecutor;
+
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+/**
+ * @brief This class provides a waitable object that is used for managing the
+ * entities (i.e. nodes and their subscriptions, timers, services, etc)
+ * added to an EventsExecutor.
+ * The add/remove node APIs are used when a node is added/removed from
+ * the associated EventsExecutor and result in setting/unsetting the
+ * events callbacks and adding timers to the timers manager.
+ *
+ * Being this class derived from Waitable, it can be used to wake up an
+ * executor thread while it's spinning.
+ * When this occurs, the execute API takes care of handling changes
+ * in the entities currently used by the executor.
+ */
+class EventsExecutorEntitiesCollector final
+  : public rclcpp::Waitable,
+  public std::enable_shared_from_this<EventsExecutorEntitiesCollector>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(EventsExecutorEntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EventsExecutorEntitiesCollector(
+    EventsExecutor * executor);
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~EventsExecutorEntitiesCollector() override;
+
+  // Initialize entities collector
+  RCLCPP_PUBLIC
+  void init();
+
+  /// Execute the waitable.
+  RCLCPP_PUBLIC
+  void
+  execute(std::shared_ptr<void> & 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.
+   * \throws std::runtime_error if wait set couldn't be cleared or filled.
+   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
+   */
+  RCLCPP_PUBLIC
+  void
+  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /**
+   * \throws std::runtime_error if it couldn't add guard condition to wait set
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Complete all available queued work without blocking.
+  /**
+   * This function checks if after the guard condition was triggered
+   * (or a spurious wakeup happened) we are really ready to execute
+   * i.e. re-collect entities
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) 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();
+
+  ///
+  /**
+   * Get the subscription shared pointer corresponding
+   * to a subscription identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(const void * subscription_id);
+
+  ///
+  /**
+   * Get the client shared pointer corresponding
+   * to a client identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(const void * client_id);
+
+  ///
+  /**
+   * Get the service shared pointer corresponding
+   * to a service identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(const void * service_id);
+
+  ///
+  /**
+   * Get the waitable shared pointer corresponding
+   * to a waitable identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(const void * waitable_id);
+
+  ///
+  /**
+   * Add a weak pointer to a waitable
+   */
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+private:
+  /// 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
+  callback_group_added_impl(
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  node_added_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  void
+  callback_group_removed_impl(
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  node_removed_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  void
+  set_entities_event_callbacks_from_map(
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  void
+  set_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  unset_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  set_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
+
+  void
+  unset_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
+
+  std::function<void(size_t)>
+  create_entity_callback(void * exec_entity_id, ExecutorEventType type);
+
+  std::function<void(size_t, int)>
+  create_waitable_callback(void * waitable_id);
+
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
+
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
+
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap weak_groups_to_guard_conditions_;
+
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
+
+  // 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_;
+
+  // Maps: entity identifiers to weak pointers from the entities registered in the executor
+  // so in the case of an event providing and ID, we can retrieve and own the corresponding
+  // entity while it performs work
+  std::unordered_map<const void *, rclcpp::SubscriptionBase::WeakPtr> weak_subscriptions_map_;
+  std::unordered_map<const void *, rclcpp::ServiceBase::WeakPtr> weak_services_map_;
+  std::unordered_map<const void *, rclcpp::ClientBase::WeakPtr> weak_clients_map_;
+  std::unordered_map<const void *, rclcpp::Waitable::WeakPtr> weak_waitables_map_;
+
+  /// Executor using this entities collector object
+  EventsExecutor * associated_executor_ = nullptr;
+  /// Instance of the timers manager used by the associated executor
+  rclcpp::experimental::TimersManager::SharedPtr timers_manager_;
+
+  // Mutex to protect vector of new nodes.
+  std::recursive_mutex reentrant_mutex_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp

@@ -33,8 +33,8 @@ enum ExecutorEventType
 
 struct ExecutorEvent
 {
-  const void * entity_key;
-  int waitable_data;
+  const void * exec_entity_id;
+  int gen_entity_id;
   ExecutorEventType type;
   size_t num_events;
 };

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_notify_waitable.hpp

@@ -0,0 +1,103 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_
+
+#include <list>
+#include <memory>
+
+#include "rcl/guard_condition.h"
+#include "rclcpp/experimental/executors/events_executor/event_waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class provides an EventWaitable that allows to
+ * wake up an EventsExecutor when a guard condition is notified.
+ */
+class EventsExecutorNotifyWaitable final : public EventWaitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutorNotifyWaitable)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EventsExecutorNotifyWaitable() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  virtual ~EventsExecutorNotifyWaitable() = default;
+
+  // The function is a no-op, since we only care of waking up the executor
+  RCLCPP_PUBLIC
+  void
+  execute(std::shared_ptr<void> & data) override
+  {
+    (void)data;
+  }
+
+  RCLCPP_PUBLIC
+  void
+  add_guard_condition(rclcpp::GuardCondition * guard_condition)
+  {
+    notify_guard_conditions_.push_back(guard_condition);
+  }
+
+  RCLCPP_PUBLIC
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    // The second argument of the callback should identify which guard condition
+    // triggered the event. However it's not relevant here as we only
+    // care about waking up the executor, so we pass a 0.
+    auto gc_callback = [callback](size_t count) {
+        callback(count, 0);
+      };
+
+    for (auto gc : notify_guard_conditions_) {
+      gc->set_on_trigger_callback(gc_callback);
+    }
+  }
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override
+  {
+    // This waitable doesn't handle any data
+    return nullptr;
+  }
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void) id;
+    return take_data();
+  }
+
+private:
+  std::list<rclcpp::GuardCondition *> notify_guard_conditions_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp

@@ -121,7 +121,7 @@ public:
 private:
   // The underlying queue implementation
   std::queue<rclcpp::experimental::executors::ExecutorEvent> event_queue_;
-  // Mutex to protect read/write access to the queue
+  // Mutex to protect the insertion/extraction of events in the queue
   mutable std::mutex mutex_;
   // Variable used to notify when an event is added to the queue
   std::condition_variable events_queue_cv_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -118,13 +118,6 @@ public:
         return nullptr;
       }
     }
-
-    if (this->buffer_->has_data()) {
-      // If there is data still to be processed, indicate to the
-      // executor or waitset by triggering the guard condition.
-      this->trigger_guard_condition();
-    }
-
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
         std::pair<ConstMessageSharedPtr, MessageUniquePtr>(

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -31,8 +31,6 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
-#include "tracetools/tracetools.h"
-
 namespace rclcpp
 {
 namespace experimental
@@ -93,10 +91,6 @@ public:
       buffer_type,
       qos_profile,
       std::make_shared<Alloc>(subscribed_type_allocator_));
-    TRACEPOINT(
-      rclcpp_ipb_to_subscription,
-      static_cast<const void *>(buffer_.get()),
-      static_cast<const void *>(this));
   }
 
   bool

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -49,13 +49,10 @@ namespace experimental
  * Because of this, they have a not-negligible impact on the performance.
  *
  * Timers execution
- * The most efficient use of this class consists in letting a TimersManager object
- * to spawn a thread where timers are monitored and optionally executed.
- * This can be controlled via the `start` and `stop` methods.
- * Ready timers can either be executed or an on_ready_callback can be used to notify
- * other entities that they are ready and need to be executed.
- * Other APIs allow to directly execute a given timer.
- *
+ * The most efficient implementation consists in letting a TimersManager object
+ * to spawn a thread where timers are monitored and periodically executed.
+ * Besides this, other APIs allow to either execute a single timer or all the
+ * currently ready ones.
  * This class assumes that the `execute_callback()` API of the stored timers is never
  * called by other entities, but it can only be called from here.
  * If this assumption is not respected, the heap property may be invalidated,
@@ -72,21 +69,13 @@ public:
    *
    * @param context custom context to be used.
    * Shared ownership of the context is held until destruction.
-   * @param on_ready_callback The timers on ready callback. The presence of this function
-   * indicates what to do when the TimersManager is running and a timer becomes ready.
-   * The TimersManager is considered "running" when the `start` method has been called.
-   * If it's callable, it will be invoked instead of the timer callback.
-   * If it's not callable, then the TimersManager will
-   * directly execute timers when they are ready.
-   * All the methods that execute a given timer (e.g. `execute_head_timer`
-   * or `execute_ready_timer`) without the TimersManager being `running`, i.e.
-   * without actually explicitly waiting for the timer to become ready, will ignore this
-   * callback.
+   * @param on_ready_callback The timers on ready callback. if not callable,
+   * this object will directly execute timers when they are ready.
    */
   RCLCPP_PUBLIC
   TimersManager(
     std::shared_ptr<rclcpp::Context> context,
-    std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
+    std::function<void(void *)> on_ready_callback = nullptr);
 
   /**
    * @brief Destruct the TimersManager object making sure to stop thread and release memory.
@@ -135,6 +124,15 @@ public:
   RCLCPP_PUBLIC
   void stop();
 
+  /**
+   * @brief Executes all the timers currently ready when the function was invoked.
+   * This function will lock all the stored timers throughout its duration.
+   * This function is thread safe.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  void execute_ready_timers();
+
   /**
    * @brief Get the number of timers that are currently ready.
    * This function is thread safe.
@@ -148,8 +146,6 @@ public:
   /**
    * @brief Executes head timer if ready.
    * This function is thread safe.
-   * This function will try to execute the timer callback regardless of whether
-   * the TimersManager on_ready_callback was passed during construction.
    *
    * @return true if head timer was ready.
    * @throws std::runtime_error if the timers thread was already running.
@@ -160,13 +156,11 @@ public:
   /**
    * @brief Executes timer identified by its ID.
    * This function is thread safe.
-   * This function will try to execute the timer callback regardless of whether
-   * the TimersManager on_ready_callback was passed during construction.
    *
    * @param timer_id the timer ID of the timer to execute
    */
   RCLCPP_PUBLIC
-  void execute_ready_timer(const rclcpp::TimerBase * timer_id);
+  void execute_ready_timer(const void * timer_id);
 
   /**
    * @brief Get the amount of time before the next timer triggers.
@@ -239,12 +233,7 @@ public:
       return removed;
     }
 
-    /**
-     * @brief Retrieve the timer identified by the key
-     * @param timer_id The ID of the timer to retrieve.
-     * @return TimerPtr if there's a timer associated with the ID, nullptr otherwise
-     */
-    TimerPtr get_timer(const rclcpp::TimerBase * timer_id)
+    TimerPtr get_timer(const void * timer_id)
     {
       for (auto & weak_timer : weak_heap_) {
         auto timer = weak_timer.lock();
@@ -466,10 +455,7 @@ public:
       std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
     }
 
-    /**
-     * @brief Helper function to clear the "on_reset_callback" on all associated timers.
-     */
-    void clear_timers_on_reset_callbacks()
+    void clear_callbacks()
     {
       for (TimerPtr & t : owned_heap_) {
         t->clear_on_reset_callback();
@@ -495,7 +481,7 @@ private:
      */
     static bool timer_greater(TimerPtr a, TimerPtr b)
     {
-      // TODO(alsora): this can cause an error if timers are using different clocks
+      // FIXME!
       return a->time_until_trigger() > b->time_until_trigger();
     }
 
@@ -527,7 +513,7 @@ private:
   void execute_ready_timers_unsafe();
 
   // Callback to be called when timer is ready
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback_ = nullptr;
+  std::function<void(void *)> on_ready_callback_ = nullptr;
 
   // Thread used to run the timers execution task
   std::thread timers_thread_;

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -84,7 +84,7 @@ public:
       options.to_rcl_subscription_options(qos),
       options.event_callbacks,
       options.use_default_callbacks,
-      DeliveredMessageKind::SERIALIZED_MESSAGE),
+      true),
     callback_(callback),
     ts_lib_(ts_lib)
   {}
@@ -123,31 +123,6 @@ public:
   RCLCPP_PUBLIC
   void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
 
-
-  // DYNAMIC TYPE ==================================================================================
-  RCLCPP_PUBLIC
-  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
-  override;
-
-  RCLCPP_PUBLIC
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
-
-  RCLCPP_PUBLIC
-  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
-  get_shared_dynamic_serialization_support() override;
-
-  RCLCPP_PUBLIC
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
-
-  RCLCPP_PUBLIC
-  void return_dynamic_message(
-    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
-
-  RCLCPP_PUBLIC
-  void handle_dynamic_message(
-    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-    const rclcpp::MessageInfo & message_info) override;
-
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
 

rclcpp/include/rclcpp/logger.hpp

@@ -170,24 +170,6 @@ public:
   RCLCPP_PUBLIC
   void
   set_level(Level level);
-
-  /// Get effective level for current logger.
-  /**
-   * The effective level is determined as the severity level of
-   * the logger if it is set, otherwise it is the first specified severity
-   * level of the logger's ancestors, starting with its closest ancestor.
-   * The ancestor hierarchy is signified by logger names being separated by dots:
-   * a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are
-   * ancestors of `x.y.z`, etc.
-   * If the level has not been set for the logger nor any of its
-   * ancestors, the default level is used.
-   *
-   * \throws rclcpp::exceptions::RCLError if any error happens.
-   * \return Level for the current logger.
-   */
-  RCLCPP_PUBLIC
-  Level
-  get_effective_level() const;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -121,19 +121,10 @@ public:
   std::atomic_bool &
   get_associated_with_executor_atomic() override;
 
-  [[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]]
   RCLCPP_PUBLIC
   rclcpp::GuardCondition &
   get_notify_guard_condition() override;
 
-  RCLCPP_PUBLIC
-  rclcpp::GuardCondition::SharedPtr
-  get_shared_notify_guard_condition() override;
-
-  RCLCPP_PUBLIC
-  void
-  trigger_notify_guard_condition() override;
-
   RCLCPP_PUBLIC
   bool
   get_use_intra_process_default() const override;
@@ -162,7 +153,7 @@ private:
 
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
-  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_;
+  rclcpp::GuardCondition notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -148,33 +148,13 @@ public:
   /**
    * For example, this should be notified when a publisher is added or removed.
    *
-   * \return the GuardCondition if it is valid, else throw runtime error
+   * \return the GuardCondition if it is valid, else thow runtime error
    */
   RCLCPP_PUBLIC
   virtual
   rclcpp::GuardCondition &
   get_notify_guard_condition() = 0;
 
-  /// Return a guard condition that should be notified when the internal node state changes.
-  /**
-   * For example, this should be notified when a publisher is added or removed.
-   *
-   * \return the GuardCondition if it is valid, else nullptr
-   */
-  RCLCPP_PUBLIC
-  virtual
-  rclcpp::GuardCondition::SharedPtr
-  get_shared_notify_guard_condition() = 0;
-
-  /// Trigger the guard condition that notifies of internal node state changes.
-  /**
-   * For example, this should be notified when a publisher is added or removed.
-   */
-  RCLCPP_PUBLIC
-  virtual
-  void
-  trigger_notify_guard_condition() = 0;
-
   /// Return the default preference for using intra process communication.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -57,8 +57,7 @@ public:
     node_namespace_(info.node_namespace),
     topic_type_(info.topic_type),
     endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
-    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile),
-    topic_type_hash_(info.topic_type_hash)
+    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
   {
     std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
   }
@@ -123,16 +122,6 @@ public:
   const rclcpp::QoS &
   qos_profile() const;
 
-  /// Get a mutable reference to the type hash of the topic endpoint.
-  RCLCPP_PUBLIC
-  rosidl_type_hash_t &
-  topic_type_hash();
-
-  /// Get a const reference to the type hash of the topic endpoint.
-  RCLCPP_PUBLIC
-  const rosidl_type_hash_t &
-  topic_type_hash() const;
-
 private:
   std::string node_name_;
   std::string node_namespace_;
@@ -140,7 +129,6 @@ private:
   rclcpp::EndpointType endpoint_type_;
   std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
   rclcpp::QoS qos_profile_;
-  rosidl_type_hash_t topic_type_hash_;
 };
 
 namespace node_interfaces

rclcpp/include/rclcpp/publisher.hpp

@@ -484,10 +484,6 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
-      rclcpp_intra_publish,
-      static_cast<const void *>(publisher_handle_.get()),
-      msg.get());
 
     ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
@@ -506,10 +502,6 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
-      rclcpp_intra_publish,
-      static_cast<const void *>(publisher_handle_.get()),
-      msg.get());
 
     ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
@@ -529,10 +521,6 @@ protected:
     if (!msg) {
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    TRACEPOINT(
-      rclcpp_intra_publish,
-      static_cast<const void *>(publisher_handle_.get()),
-      msg.get());
 
     return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
              AllocatorT>(

rclcpp/include/rclcpp/rclcpp.hpp

@@ -117,18 +117,6 @@
  * - Allocator related items:
  *   - rclcpp/allocator/allocator_common.hpp
  *   - rclcpp/allocator/allocator_deleter.hpp
- * - Dynamic typesupport wrappers
- *   - rclcpp::dynamic_typesupport::DynamicMessage
- *   - rclcpp::dynamic_typesupport::DynamicMessageType
- *   - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder
- *   - rclcpp::dynamic_typesupport::DynamicSerializationSupport
- *   - rclcpp/dynamic_typesupport/dynamic_message.hpp
- *   - rclcpp/dynamic_typesupport/dynamic_message_type.hpp
- *   - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp
- *   - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp
- * - Dynamic typesupport
- *   - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
- *   - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
  * - Generic publisher
  *   - rclcpp::Node::create_generic_publisher()
  *   - rclcpp::GenericPublisher

rclcpp/include/rclcpp/scope_exit.hpp

@@ -0,0 +1,56 @@
+// Copyright 2015 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.
+
+// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/
+// But I changed the lambda to include by reference rather than value, see:
+// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873
+
+#ifndef RCLCPP__SCOPE_EXIT_HPP_
+#define RCLCPP__SCOPE_EXIT_HPP_
+
+// TODO(christophebedard) remove this header completely in I-turtle
+
+#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead
+
+#include <functional>
+
+#include "rclcpp/macros.hpp"
+
+namespace rclcpp
+{
+
+template<typename Callable>
+struct ScopeExit
+{
+  explicit ScopeExit(Callable callable)
+  : callable_(callable) {}
+  ~ScopeExit() {callable_();}
+
+private:
+  Callable callable_;
+};
+
+template<typename Callable>
+ScopeExit<Callable>
+make_scope_exit(Callable callable)
+{
+  return ScopeExit<Callable>(callable);
+}
+
+}  // namespace rclcpp
+
+#define RCLCPP_SCOPE_EXIT(code) \
+  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;})
+
+#endif  // RCLCPP__SCOPE_EXIT_HPP_

rclcpp/include/rclcpp/subscription.hpp

@@ -144,7 +144,7 @@ public:
       // NOTE(methylDragon): Passing these args separately is necessary for event binding
       options.event_callbacks,
       options.use_default_callbacks,
-      callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE),  // NOLINT
+      callback.is_serialized_message_callback()),
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
@@ -388,57 +388,6 @@ public:
     return any_callback_.use_take_shared_method();
   }
 
-  // DYNAMIC TYPE ==================================================================================
-  // TODO(methylDragon): Reorder later
-  // TODO(methylDragon): Implement later...
-  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
-  get_shared_dynamic_message_type() override
-  {
-    throw rclcpp::exceptions::UnimplementedError(
-            "get_shared_dynamic_message_type is not implemented for Subscription");
-  }
-
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-  get_shared_dynamic_message() override
-  {
-    throw rclcpp::exceptions::UnimplementedError(
-            "get_shared_dynamic_message is not implemented for Subscription");
-  }
-
-  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
-  get_shared_dynamic_serialization_support() override
-  {
-    throw rclcpp::exceptions::UnimplementedError(
-            "get_shared_dynamic_serialization_support is not implemented for Subscription");
-  }
-
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-  create_dynamic_message() override
-  {
-    throw rclcpp::exceptions::UnimplementedError(
-            "create_dynamic_message is not implemented for Subscription");
-  }
-
-  void
-  return_dynamic_message(
-    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
-  {
-    (void) message;
-    throw rclcpp::exceptions::UnimplementedError(
-            "return_dynamic_message is not implemented for Subscription");
-  }
-
-  void
-  handle_dynamic_message(
-    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-    const rclcpp::MessageInfo & message_info) override
-  {
-    (void) message;
-    (void) message_info;
-    throw rclcpp::exceptions::UnimplementedError(
-            "handle_dynamic_message is not implemented for Subscription");
-  }
-
 private:
   RCLCPP_DISABLE_COPY(Subscription)
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -31,9 +31,6 @@
 
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
@@ -63,27 +60,6 @@ namespace experimental
 class IntraProcessManager;
 }  // namespace experimental
 
-/// The kind of message that the subscription delivers in its callback, used by the executor
-/**
- * This enum needs to exist because the callback handle is not accessible to the executor's scope.
- *
- * "Kind" is used since what is being delivered is a category of messages, for example, there are
- * different ROS message types that can be delivered, but they're all ROS messages.
- *
- * As a concrete example, all of the following callbacks will be considered ROS_MESSAGE for
- * DeliveredMessageKind:
- *   - void callback(const std_msgs::msg::String &)
- *   - void callback(const std::string &)  // type adaption
- *   - void callback(std::unique_ptr<std_msgs::msg::String>)
- */
-enum class DeliveredMessageKind : uint8_t
-{
-  INVALID = 0,
-  ROS_MESSAGE = 1,  // The subscription delivers a ROS message to its callback
-  SERIALIZED_MESSAGE = 2,  // The subscription delivers a serialized message to its callback
-  DYNAMIC_MESSAGE = 3,  // The subscription delivers a dynamic message to its callback
-};
-
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
@@ -100,8 +76,7 @@ public:
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options Options for the subscription.
-   * \param[in] delivered_message_kind Enum flag to change how the message will be received and
-   *            delivered
+   * \param[in] is_serialized is true if the message will be delivered still serialized
    */
   RCLCPP_PUBLIC
   SubscriptionBase(
@@ -111,7 +86,7 @@ public:
     const rcl_subscription_options_t & subscription_options,
     const SubscriptionEventCallbacks & event_callbacks,
     bool use_default_callbacks,
-    DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE);
+    bool is_serialized = false);
 
   /// Destructor.
   RCLCPP_PUBLIC
@@ -260,14 +235,6 @@ public:
   bool
   is_serialized() const;
 
-  /// Return the type of the subscription.
-  /**
-   * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
-   */
-  RCLCPP_PUBLIC
-  DeliveredMessageKind
-  get_subscription_type() const;
-
   /// Get matching publisher count.
   /** \return The number of publishers on this topic. */
   RCLCPP_PUBLIC
@@ -568,49 +535,6 @@ public:
   rclcpp::ContentFilterOptions
   get_content_filter() const;
 
-  // DYNAMIC TYPE ==================================================================================
-  // TODO(methylDragon): Reorder later
-  RCLCPP_PUBLIC
-  virtual
-  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
-  get_shared_dynamic_message_type() = 0;
-
-  RCLCPP_PUBLIC
-  virtual
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-  get_shared_dynamic_message() = 0;
-
-  RCLCPP_PUBLIC
-  virtual
-  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
-  get_shared_dynamic_serialization_support() = 0;
-
-  /// Borrow a new serialized message (this clones!)
-  /** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */
-  RCLCPP_PUBLIC
-  virtual
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-  create_dynamic_message() = 0;
-
-  RCLCPP_PUBLIC
-  virtual
-  void
-  return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0;
-
-  RCLCPP_PUBLIC
-  virtual
-  void
-  handle_dynamic_message(
-    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-    const rclcpp::MessageInfo & message_info) = 0;
-
-  RCLCPP_PUBLIC
-  bool
-  take_dynamic_message(
-    rclcpp::dynamic_typesupport::DynamicMessage & message_out,
-    rclcpp::MessageInfo & message_info_out);
-  // ===============================================================================================
-
 protected:
   template<typename EventCallbackT>
   void
@@ -663,7 +587,7 @@ private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
   rosidl_message_type_support_t type_support_;
-  DeliveredMessageKind delivered_message_type_;
+  bool is_serialized_;
 
   std::atomic<bool> subscription_in_use_by_wait_set_{false};
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};

rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp

@@ -104,7 +104,7 @@ protected:
       // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
       rcl_get_default_allocator());
     if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to create wait set");
+      rclcpp::exceptions::throw_from_rcl_error(ret);
     }
 
     // (Re)build the wait set for the first time.
@@ -192,7 +192,8 @@ protected:
       size_t services_from_waitables = 0;
       size_t events_from_waitables = 0;
       for (const auto & waitable_entry : waitables) {
-        if (!waitable_entry.waitable) {
+        auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+        if (nullptr == waitable_ptr_pair.second) {
           // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
           if (HasStrongOwnership) {
             // This will not happen in fixed sized storage, as it holds
@@ -203,13 +204,13 @@ protected:
           needs_pruning_ = true;
           continue;
         }
-        const auto & waitable = waitable_entry.waitable;
-        subscriptions_from_waitables += waitable->get_number_of_ready_subscriptions();
-        guard_conditions_from_waitables += waitable->get_number_of_ready_guard_conditions();
-        timers_from_waitables += waitable->get_number_of_ready_timers();
-        clients_from_waitables += waitable->get_number_of_ready_clients();
-        services_from_waitables += waitable->get_number_of_ready_services();
-        events_from_waitables += waitable->get_number_of_ready_events();
+        rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
+        subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
+        guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
+        timers_from_waitables += waitable.get_number_of_ready_timers();
+        clients_from_waitables += waitable.get_number_of_ready_clients();
+        services_from_waitables += waitable.get_number_of_ready_services();
+        events_from_waitables += waitable.get_number_of_ready_events();
       }
       rcl_ret_t ret = rcl_wait_set_resize(
         &rcl_wait_set_,
@@ -221,7 +222,7 @@ protected:
         events_from_waitables
       );
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
       was_resized = true;
       // Assumption: the calling code ensures this function is not called
@@ -237,13 +238,15 @@ protected:
     if (!was_resized) {
       rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear the wait set");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     }
 
     // Add subscriptions.
     for (const auto & subscription_entry : subscriptions) {
-      if (!subscription_entry.subscription) {
+      auto subscription_ptr_pair =
+        get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
+      if (nullptr == subscription_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -254,13 +257,12 @@ protected:
         needs_pruning_ = true;
         continue;
       }
-
       rcl_ret_t ret = rcl_wait_set_add_subscription(
         &rcl_wait_set_,
-        subscription_entry.subscription->get_subscription_handle().get(),
+        subscription_ptr_pair.second->get_subscription_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     }
 
@@ -269,7 +271,8 @@ protected:
       [this](const auto & inner_guard_conditions)
       {
         for (const auto & guard_condition : inner_guard_conditions) {
-          if (!guard_condition) {
+          auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition);
+          if (nullptr == guard_condition_ptr_pair.second) {
             // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
             if (HasStrongOwnership) {
               // This will not happen in fixed sized storage, as it holds
@@ -282,10 +285,10 @@ protected:
           }
           rcl_ret_t ret = rcl_wait_set_add_guard_condition(
             &rcl_wait_set_,
-            &guard_condition->get_rcl_guard_condition(),
+            &guard_condition_ptr_pair.second->get_rcl_guard_condition(),
             nullptr);
           if (RCL_RET_OK != ret) {
-            rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
+            rclcpp::exceptions::throw_from_rcl_error(ret);
           }
         }
       };
@@ -298,7 +301,8 @@ protected:
 
     // Add timers.
     for (const auto & timer : timers) {
-      if (!timer) {
+      auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer);
+      if (nullptr == timer_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -311,16 +315,17 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_timer(
         &rcl_wait_set_,
-        timer->get_timer_handle().get(),
+        timer_ptr_pair.second->get_timer_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     }
 
     // Add clients.
     for (const auto & client : clients) {
-      if (!client) {
+      auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client);
+      if (nullptr == client_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -333,17 +338,17 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_client(
         &rcl_wait_set_,
-        client->get_client_handle().get(),
+        client_ptr_pair.second->get_client_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
         rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     }
 
-
     // Add services.
     for (const auto & service : services) {
-      if (!service) {
+      auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service);
+      if (nullptr == service_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -356,16 +361,17 @@ protected:
       }
       rcl_ret_t ret = rcl_wait_set_add_service(
         &rcl_wait_set_,
-        service->get_service_handle().get(),
+        service_ptr_pair.second->get_service_handle().get(),
         nullptr);
       if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     }
 
     // Add waitables.
     for (auto & waitable_entry : waitables) {
-      if (!waitable_entry.waitable) {
+      auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
+      if (nullptr == waitable_ptr_pair.second) {
         // In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
         if (HasStrongOwnership) {
           // This will not happen in fixed sized storage, as it holds
@@ -376,7 +382,8 @@ protected:
         needs_pruning_ = true;
         continue;
       }
-      waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_);
+      rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
+      waitable.add_to_wait_set(&rcl_wait_set_);
     }
   }
 

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -204,19 +204,15 @@ public:
   void
   storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
   {
-    this->storage_acquire_ownerships();
-
     this->storage_rebuild_rcl_wait_set_with_sets(
-      shared_subscriptions_,
-      shared_guard_conditions_,
+      subscriptions_,
+      guard_conditions_,
       extra_guard_conditions,
-      shared_timers_,
-      shared_clients_,
-      shared_services_,
-      shared_waitables_
+      timers_,
+      clients_,
+      services_,
+      waitables_
     );
-
-    this->storage_release_ownerships();
   }
 
   template<class EntityT, class SequenceOfEntitiesT>
@@ -411,7 +407,6 @@ public:
         }
       };
     // Lock all the weak pointers and hold them until released.
-    lock_all(subscriptions_, shared_subscriptions_);
     lock_all(guard_conditions_, shared_guard_conditions_);
     lock_all(timers_, shared_timers_);
     lock_all(clients_, shared_clients_);
@@ -443,7 +438,6 @@ public:
           shared_ptr.reset();
         }
       };
-    reset_all(shared_subscriptions_);
     reset_all(shared_guard_conditions_);
     reset_all(shared_timers_);
     reset_all(shared_clients_);

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

@@ -290,7 +290,7 @@ protected:
         return create_wait_result(WaitResultKind::Empty);
       } else {
         // Some other error case, throw.
-        rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed");
+        rclcpp::exceptions::throw_from_rcl_error(ret);
       }
     } while (should_loop());
 

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>20.0.0</version>
+  <version>19.3.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
@@ -39,7 +39,6 @@
   <depend>rcpputils</depend>
   <depend>rcutils</depend>
   <depend>rmw</depend>
-  <depend>rosidl_dynamic_typesupport</depend>
   <depend>statistics_msgs</depend>
   <depend>tracetools</depend>
 

rclcpp/src/rclcpp/any_executable.cpp

@@ -16,16 +16,13 @@
 
 using rclcpp::AnyExecutable;
 
-RCLCPP_PUBLIC
 AnyExecutable::AnyExecutable()
 : subscription(nullptr),
   timer(nullptr),
   service(nullptr),
   client(nullptr),
-  waitable(nullptr),
   callback_group(nullptr),
-  node_base(nullptr),
-  data(nullptr)
+  node_base(nullptr)
 {}
 
 AnyExecutable::~AnyExecutable()

rclcpp/src/rclcpp/callback_group.cpp

@@ -31,12 +31,10 @@ using rclcpp::CallbackGroupType;
 
 CallbackGroup::CallbackGroup(
   CallbackGroupType group_type,
-  std::function<rclcpp::Context::SharedPtr(void)> get_context,
   bool automatically_add_to_executor_with_node)
 : type_(group_type), associated_with_executor_(false),
   can_be_taken_from_(true),
-  automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node),
-  get_context_(get_context)
+  automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
 CallbackGroup::~CallbackGroup()
@@ -56,17 +54,6 @@ CallbackGroup::type() const
   return type_;
 }
 
-size_t
-CallbackGroup::size() const
-{
-  return
-    subscription_ptrs_.size() +
-    service_ptrs_.size() +
-    client_ptrs_.size() +
-    timer_ptrs_.size() +
-    waitable_ptrs_.size();
-}
-
 void CallbackGroup::collect_all_ptrs(
   std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
   std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
@@ -124,7 +111,6 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
-// \TODO(mjcarroll) Deprecated, remove on tock
 rclcpp::GuardCondition::SharedPtr
 CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
 {
@@ -143,29 +129,6 @@ CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr conte
   return notify_guard_condition_;
 }
 
-rclcpp::GuardCondition::SharedPtr
-CallbackGroup::get_notify_guard_condition()
-{
-  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
-  if (!this->get_context_) {
-    throw std::runtime_error("Callback group was created without context and not passed context");
-  }
-  auto context_ptr = this->get_context_();
-  if (context_ptr && context_ptr->is_valid()) {
-    if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
-      if (associated_with_executor_) {
-        trigger_notify_guard_condition();
-      }
-      notify_guard_condition_ = nullptr;
-    }
-    if (!notify_guard_condition_) {
-      notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
-    }
-    return notify_guard_condition_;
-  }
-  return nullptr;
-}
-
 void
 CallbackGroup::trigger_notify_guard_condition()
 {

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp

@@ -1,40 +0,0 @@
-// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_type.h>
-#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
-#include <rosidl_dynamic_typesupport/api/serialization_support.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rcl/allocator.h"
-#include "rcl/types.h"
-#include "rcutils/logging_macros.h"
-
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/exceptions.hpp"
-
-
-using rclcpp::dynamic_typesupport::DynamicMessage;
-using rclcpp::dynamic_typesupport::DynamicMessageType;
-using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-DynamicMessage::~DynamicMessage()
-{}  // STUBBED

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp

@@ -1,38 +0,0 @@
-// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_data.h>
-#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
-#include <rosidl_dynamic_typesupport/api/serialization_support.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rcutils/logging_macros.h"
-
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/exceptions.hpp"
-
-
-using rclcpp::dynamic_typesupport::DynamicMessage;
-using rclcpp::dynamic_typesupport::DynamicMessageType;
-using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-DynamicMessageType::~DynamicMessageType()
-{}  // STUBBED

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp

@@ -1,37 +0,0 @@
-// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_data.h>
-#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
-#include <rosidl_dynamic_typesupport/api/serialization_support.h>
-#include <rosidl_dynamic_typesupport/types.h>
-
-#include <memory>
-#include <string>
-
-#include "rcutils/logging_macros.h"
-
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/exceptions.hpp"
-
-using rclcpp::dynamic_typesupport::DynamicMessage;
-using rclcpp::dynamic_typesupport::DynamicMessageType;
-using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder()
-{}  // STUBBED

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp

@@ -1,49 +0,0 @@
-// Copyright 2023 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 <rosidl_dynamic_typesupport/identifier.h>
-#include <rosidl_dynamic_typesupport/types.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-#include <rosidl_runtime_c/type_description_utils.h>
-#include <rosidl_runtime_c/type_description/type_description__functions.h>
-#include <rosidl_runtime_c/type_description/type_description__struct.h>
-#include <rosidl_runtime_c/type_description/type_source__functions.h>
-#include <rosidl_runtime_c/type_description/type_source__struct.h>
-
-#include <memory>
-#include <string>
-
-#include "rcl/allocator.h"
-#include "rcl/dynamic_message_type_support.h"
-#include "rcl/type_hash.h"
-#include "rcl/types.h"
-#include "rcutils/logging_macros.h"
-#include "rcutils/types/rcutils_ret.h"
-#include "rmw/dynamic_message_type_support.h"
-
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-using rclcpp::dynamic_typesupport::DynamicMessage;
-using rclcpp::dynamic_typesupport::DynamicMessageType;
-using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-DynamicMessageTypeSupport::~DynamicMessageTypeSupport()
-{}  // STUBBED

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp

@@ -1,46 +0,0 @@
-// Copyright 2023 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 <rcl/allocator.h>
-#include <rcutils/logging_macros.h>
-#include <rmw/dynamic_message_type_support.h>
-#include <rmw/ret_types.h>
-#include <rosidl_dynamic_typesupport/api/serialization_support.h>
-
-#include <memory>
-#include <string>
-
-#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
-#include "rclcpp/exceptions.hpp"
-
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-// CONSTRUCTION ====================================================================================
-DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator)
-: DynamicSerializationSupport::DynamicSerializationSupport("", allocator)
-{
-  throw std::runtime_error("Unimplemented");
-}
-
-DynamicSerializationSupport::DynamicSerializationSupport(
-  const std::string & /*serialization_library_name*/,
-  rcl_allocator_t /*allocator*/)
-: rosidl_serialization_support_(
-    rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())
-{
-  throw std::runtime_error("Unimplemented");
-}
-
-DynamicSerializationSupport::~DynamicSerializationSupport()
-{}  // STUBBED

rclcpp/src/rclcpp/executors/executor_entities_collection.cpp

@@ -1,228 +0,0 @@
-// Copyright 2023 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/executor_entities_collection.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-bool ExecutorEntitiesCollection::empty() const
-{
-  return
-    subscriptions.empty() &&
-    timers.empty() &&
-    guard_conditions.empty() &&
-    clients.empty() &&
-    services.empty() &&
-    waitables.empty();
-}
-
-void ExecutorEntitiesCollection::clear()
-{
-  subscriptions.clear();
-  timers.clear();
-  guard_conditions.clear();
-  clients.clear();
-  services.clear();
-  waitables.clear();
-}
-
-void
-build_entities_collection(
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
-  ExecutorEntitiesCollection & collection)
-{
-  collection.clear();
-
-  for (auto weak_group_ptr : callback_groups) {
-    auto group_ptr = weak_group_ptr.lock();
-    if (!group_ptr) {
-      continue;
-    }
-
-    if (group_ptr->can_be_taken_from().load()) {
-      group_ptr->collect_all_ptrs(
-        [&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
-          collection.subscriptions.insert(
-            {
-              subscription->get_subscription_handle().get(),
-              {subscription, weak_group_ptr}
-            });
-        },
-        [&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
-          collection.services.insert(
-            {
-              service->get_service_handle().get(),
-              {service, weak_group_ptr}
-            });
-        },
-        [&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
-          collection.clients.insert(
-            {
-              client->get_client_handle().get(),
-              {client, weak_group_ptr}
-            });
-        },
-        [&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
-          collection.timers.insert(
-            {
-              timer->get_timer_handle().get(),
-              {timer, weak_group_ptr}
-            });
-        },
-        [&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
-          collection.waitables.insert(
-            {
-              waitable.get(),
-              {waitable, weak_group_ptr}
-            });
-        }
-      );
-    }
-  }
-}
-
-size_t
-ready_executables(
-  const ExecutorEntitiesCollection & collection,
-  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
-  std::deque<rclcpp::AnyExecutable> & executables
-)
-{
-  size_t added = 0;
-  if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
-    return added;
-  }
-  auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
-
-  // Cache shared pointers to groups to avoid extra work re-locking them
-  std::map<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::CallbackGroup::SharedPtr,
-    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> group_map;
-
-  auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr)
-    {
-      if (group_map.count(weak_cbg_ptr) == 0) {
-        group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()});
-      }
-      return group_map.find(weak_cbg_ptr)->second;
-    };
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
-    if (nullptr == rcl_wait_set.timers[ii]) {continue;}
-    auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
-    if (entity_iter != collection.timers.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
-        continue;
-      }
-      if (!entity->call()) {
-        continue;
-      }
-      rclcpp::AnyExecutable exec;
-      exec.timer = entity;
-      exec.callback_group = group_info;
-      executables.push_back(exec);
-      added++;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
-    if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
-    auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
-    if (entity_iter != collection.subscriptions.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
-        continue;
-      }
-      rclcpp::AnyExecutable exec;
-      exec.subscription = entity;
-      exec.callback_group = group_info;
-      executables.push_back(exec);
-      added++;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
-    if (nullptr == rcl_wait_set.services[ii]) {continue;}
-    auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
-    if (entity_iter != collection.services.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
-        continue;
-      }
-      rclcpp::AnyExecutable exec;
-      exec.service = entity;
-      exec.callback_group = group_info;
-      executables.push_back(exec);
-      added++;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
-    if (nullptr == rcl_wait_set.clients[ii]) {continue;}
-    auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
-    if (entity_iter != collection.clients.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
-        continue;
-      }
-      rclcpp::AnyExecutable exec;
-      exec.client = entity;
-      exec.callback_group = group_info;
-      executables.push_back(exec);
-      added++;
-    }
-  }
-
-  for (auto & [handle, entry] : collection.waitables) {
-    auto waitable = entry.entity.lock();
-    if (!waitable) {
-      continue;
-    }
-    if (!waitable->is_ready(&rcl_wait_set)) {
-      continue;
-    }
-    auto group_info = group_cache(entry.callback_group);
-    if (group_info && !group_info->can_be_taken_from().load()) {
-      continue;
-    }
-    rclcpp::AnyExecutable exec;
-    exec.waitable = waitable;
-    exec.callback_group = group_info;
-    exec.data = waitable->take_data();
-    executables.push_back(exec);
-    added++;
-  }
-  return added;
-}
-}  // namespace executors
-}  // namespace rclcpp

rclcpp/src/rclcpp/executors/executor_entities_collector.cpp

@@ -1,420 +0,0 @@
-// Copyright 2023 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 <set>
-
-#include "rclcpp/executors/executor_entities_collector.hpp"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-
-ExecutorEntitiesCollector::ExecutorEntitiesCollector(
-  std::shared_ptr<ExecutorNotifyWaitable> notify_waitable)
-: notify_waitable_(notify_waitable)
-{
-}
-
-ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
-{
-  for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) {
-    weak_node_it = remove_weak_node(weak_node_it);
-  }
-
-  for (auto weak_group_it = automatically_added_groups_.begin();
-    weak_group_it != automatically_added_groups_.end(); )
-  {
-    weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_);
-  }
-
-  for (auto weak_group_it = manually_added_groups_.begin();
-    weak_group_it != manually_added_groups_.end(); )
-  {
-    weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_);
-  }
-
-  for (auto weak_node_ptr : pending_added_nodes_) {
-    auto node_ptr = weak_node_ptr.lock();
-    if (node_ptr) {
-      node_ptr->get_associated_with_executor_atomic().store(false);
-    }
-  }
-  pending_added_nodes_.clear();
-  pending_removed_nodes_.clear();
-
-  for (auto weak_group_ptr : pending_manually_added_groups_) {
-    auto group_ptr = weak_group_ptr.lock();
-    if (group_ptr) {
-      group_ptr->get_associated_with_executor_atomic().store(false);
-    }
-  }
-  pending_manually_added_groups_.clear();
-  pending_manually_removed_groups_.clear();
-}
-
-bool
-ExecutorEntitiesCollector::has_pending() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return pending_manually_added_groups_.size() != 0 ||
-         pending_manually_removed_groups_.size() != 0 ||
-         pending_added_nodes_.size() != 0 ||
-         pending_removed_nodes_.size() != 0;
-}
-
-void
-ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  // 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(
-            std::string("Node '") + node_ptr->get_fully_qualified_name() +
-            "' has already been added to an executor.");
-  }
-
-  std::lock_guard<std::mutex> lock(mutex_);
-  bool associated = weak_nodes_.count(node_ptr) != 0;
-  bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
-  bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
-
-  if ((associated || add_queued) && !remove_queued) {
-    throw std::runtime_error(
-            std::string("Node '") + node_ptr->get_fully_qualified_name() +
-            "' has already been added to this executor.");
-  }
-
-  this->pending_added_nodes_.insert(node_ptr);
-}
-
-void
-ExecutorEntitiesCollector::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
-{
-  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-  if (!has_executor.exchange(false)) {
-    throw std::runtime_error(
-            std::string("Node '") + node_ptr->get_fully_qualified_name() +
-            "' needs to be associated with an executor.");
-  }
-
-  std::lock_guard<std::mutex> lock(mutex_);
-  bool associated = weak_nodes_.count(node_ptr) != 0;
-  bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
-  bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
-
-  if (!(associated || add_queued) || remove_queued) {
-    throw std::runtime_error(
-            std::string("Node '") + node_ptr->get_fully_qualified_name() +
-            "' needs to be associated with this executor.");
-  }
-
-  this->pending_removed_nodes_.insert(node_ptr);
-}
-
-void
-ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
-{
-  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.");
-  }
-
-  std::lock_guard<std::mutex> lock(mutex_);
-  bool associated = manually_added_groups_.count(group_ptr) != 0;
-  bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
-  bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
-
-  if ((associated || add_queued) && !remove_queued) {
-    throw std::runtime_error("Callback group has already been added to this executor.");
-  }
-
-  this->pending_manually_added_groups_.insert(group_ptr);
-}
-
-void
-ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
-{
-  if (!group_ptr->get_associated_with_executor_atomic().load()) {
-    throw std::runtime_error("Callback group needs to be associated with an executor.");
-  }
-  /**
-   * TODO(mjcarroll): The callback groups, being created by a node, should never outlive
-   * the node. Since we haven't historically enforced this, turning this on may cause
-   * previously-functional code to fail.
-   * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
-   * when we can guarantee node/group lifetimes.
-  if (!group_ptr->has_valid_node()) {
-    throw std::runtime_error("Node must not be deleted before its callback group(s).");
-  }
-  */
-  auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr);
-  std::lock_guard<std::mutex> lock(mutex_);
-  bool associated = manually_added_groups_.count(group_ptr) != 0;
-  bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
-  bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
-
-  if (!(associated || add_queued) || remove_queued) {
-    throw std::runtime_error("Callback group needs to be associated with this executor.");
-  }
-
-  this->pending_manually_removed_groups_.insert(group_ptr);
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-ExecutorEntitiesCollector::get_all_callback_groups() const
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  std::lock_guard<std::mutex> lock(mutex_);
-  for (const auto & group_ptr : manually_added_groups_) {
-    groups.push_back(group_ptr);
-  }
-  for (auto const & group_ptr : automatically_added_groups_) {
-    groups.push_back(group_ptr);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-ExecutorEntitiesCollector::get_manually_added_callback_groups() const
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  std::lock_guard<std::mutex> lock(mutex_);
-  for (const auto & group_ptr : manually_added_groups_) {
-    groups.push_back(group_ptr);
-  }
-  return groups;
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-ExecutorEntitiesCollector::get_automatically_added_callback_groups() const
-{
-  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  std::lock_guard<std::mutex> lock(mutex_);
-  for (auto const & group_ptr : automatically_added_groups_) {
-    groups.push_back(group_ptr);
-  }
-  return groups;
-}
-
-void
-ExecutorEntitiesCollector::update_collections()
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  this->process_queues();
-  this->add_automatically_associated_callback_groups(this->weak_nodes_);
-  this->prune_invalid_nodes_and_groups();
-}
-
-ExecutorEntitiesCollector::NodeCollection::iterator
-ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node)
-{
-  // Disassociate the guard condition from the executor notify waitable
-  auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node);
-  if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) {
-    this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
-    weak_nodes_to_guard_conditions_.erase(guard_condition_it);
-  }
-
-  // Mark the node as disassociated (if the node is still valid)
-  auto node_ptr = weak_node->lock();
-  if (node_ptr) {
-    std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
-    has_executor.store(false);
-  }
-
-  // Remove the node from tracked nodes
-  return weak_nodes_.erase(weak_node);
-}
-
-ExecutorEntitiesCollector::CallbackGroupCollection::iterator
-ExecutorEntitiesCollector::remove_weak_callback_group(
-  CallbackGroupCollection::iterator weak_group_it,
-  CallbackGroupCollection & collection
-)
-{
-  // Disassociate the guard condition from the executor notify waitable
-  auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it);
-  if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
-    this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
-    weak_groups_to_guard_conditions_.erase(guard_condition_it);
-  }
-
-  // Mark the node as disassociated (if the group is still valid)
-  auto group_ptr = weak_group_it->lock();
-  if (group_ptr) {
-    /**
-     * TODO(mjcarroll): The callback groups, being created by a node, should never outlive
-     * the node. Since we haven't historically enforced this, turning this on may cause
-     * previously-functional code to fail.
-     * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
-     * when we can guarantee node/group lifetimes.
-    if (!group_ptr->has_valid_node()) {
-      throw std::runtime_error("Node must not be deleted before its callback group(s).");
-    }
-    */
-    std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
-    has_executor.store(false);
-  }
-
-  // Remove the node from tracked nodes
-  return collection.erase(weak_group_it);
-}
-
-void
-ExecutorEntitiesCollector::add_callback_group_to_collection(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  CallbackGroupCollection & collection)
-{
-  auto iter = collection.insert(group_ptr);
-  if (iter.second == false) {
-    throw std::runtime_error("Callback group has already been added to this executor.");
-  }
-
-  // Store node guard condition in map and add it to the notify waitable
-  auto group_guard_condition = group_ptr->get_notify_guard_condition();
-  weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
-  this->notify_waitable_->add_guard_condition(group_guard_condition);
-}
-
-void
-ExecutorEntitiesCollector::process_queues()
-{
-  for (auto weak_node_ptr : pending_added_nodes_) {
-    auto node_ptr = weak_node_ptr.lock();
-    if (!node_ptr) {
-      continue;
-    }
-    weak_nodes_.insert(weak_node_ptr);
-    this->add_automatically_associated_callback_groups({weak_node_ptr});
-
-    // Store node guard condition in map and add it to the notify waitable
-    auto node_guard_condition = node_ptr->get_shared_notify_guard_condition();
-    weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition});
-    this->notify_waitable_->add_guard_condition(node_guard_condition);
-  }
-  pending_added_nodes_.clear();
-
-  for (auto weak_node_ptr : pending_removed_nodes_) {
-    auto node_it = weak_nodes_.find(weak_node_ptr);
-    if (node_it != weak_nodes_.end()) {
-      remove_weak_node(node_it);
-    } else {
-      // The node may have been destroyed and removed from the colletion before
-      // we processed the queues.  Don't throw if the pointer is already expired.
-      if (!weak_node_ptr.expired()) {
-        throw std::runtime_error("Node needs to be associated with this executor.");
-      }
-    }
-
-    auto node_ptr = weak_node_ptr.lock();
-    if (node_ptr) {
-      for (auto group_it = automatically_added_groups_.begin();
-        group_it != automatically_added_groups_.end(); )
-      {
-        auto group_ptr = group_it->lock();
-        if (node_ptr->callback_group_in_node(group_ptr)) {
-          group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
-        } else {
-          ++group_it;
-        }
-      }
-    }
-  }
-  pending_removed_nodes_.clear();
-
-  for (auto weak_group_ptr : pending_manually_added_groups_) {
-    auto group_ptr = weak_group_ptr.lock();
-    if (group_ptr) {
-      this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
-    }
-  }
-  pending_manually_added_groups_.clear();
-
-  for (auto weak_group_ptr : pending_manually_removed_groups_) {
-    auto group_ptr = weak_group_ptr.lock();
-    if (group_ptr) {
-      auto group_it = manually_added_groups_.find(group_ptr);
-      if (group_it != manually_added_groups_.end()) {
-        remove_weak_callback_group(group_it, manually_added_groups_);
-      } else {
-        throw std::runtime_error(
-                "Attempting to remove a callback group not added to this executor.");
-      }
-    }
-  }
-  pending_manually_removed_groups_.clear();
-}
-
-void
-ExecutorEntitiesCollector::add_automatically_associated_callback_groups(
-  const NodeCollection & nodes_to_check)
-{
-  for (auto & weak_node : nodes_to_check) {
-    auto node = weak_node.lock();
-    if (node) {
-      node->for_each_callback_group(
-        [this, node](rclcpp::CallbackGroup::SharedPtr group_ptr)
-        {
-          if (!group_ptr->get_associated_with_executor_atomic().load() &&
-          group_ptr->automatically_add_to_executor_with_node())
-          {
-            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.");
-            }
-            this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_);
-          }
-        });
-    }
-  }
-}
-
-void
-ExecutorEntitiesCollector::prune_invalid_nodes_and_groups()
-{
-  for (auto node_it = weak_nodes_.begin();
-    node_it != weak_nodes_.end(); )
-  {
-    if (node_it->expired()) {
-      node_it = remove_weak_node(node_it);
-    } else {
-      node_it++;
-    }
-  }
-  for (auto group_it = automatically_added_groups_.begin();
-    group_it != automatically_added_groups_.end(); )
-  {
-    if (group_it->expired()) {
-      group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
-    } else {
-      group_it++;
-    }
-  }
-  for (auto group_it = manually_added_groups_.begin();
-    group_it != manually_added_groups_.end(); )
-  {
-    if (group_it->expired()) {
-      group_it = remove_weak_callback_group(group_it, manually_added_groups_);
-    } else {
-      group_it++;
-    }
-  }
-}
-
-}  // namespace executors
-}  // namespace rclcpp

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -1,181 +0,0 @@
-// Copyright 2023 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 <iostream>
-
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
-
-namespace rclcpp
-{
-namespace executors
-{
-
-ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback)
-: execute_callback_(on_execute_callback)
-{
-}
-
-ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other)
-: ExecutorNotifyWaitable(other.execute_callback_)
-{
-  this->notify_guard_conditions_ = other.notify_guard_conditions_;
-}
-
-ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other)
-{
-  if (this != &other) {
-    this->execute_callback_ = other.execute_callback_;
-    this->notify_guard_conditions_ = other.notify_guard_conditions_;
-  }
-  return *this;
-}
-
-void
-ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  for (auto weak_guard_condition : this->notify_guard_conditions_) {
-    auto guard_condition = weak_guard_condition.lock();
-    if (!guard_condition) {continue;}
-
-    rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition();
-
-    rcl_ret_t ret = rcl_wait_set_add_guard_condition(
-      wait_set, cond, NULL);
-
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret, "failed to add guard condition to wait set");
-    }
-  }
-}
-
-bool
-ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-
-  bool any_ready = false;
-  for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) {
-    auto rcl_guard_condition = wait_set->guard_conditions[ii];
-
-    if (nullptr == rcl_guard_condition) {
-      continue;
-    }
-    for (auto weak_guard_condition : this->notify_guard_conditions_) {
-      auto guard_condition = weak_guard_condition.lock();
-      if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) {
-        any_ready = true;
-      }
-    }
-  }
-  return any_ready;
-}
-
-void
-ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
-{
-  (void) data;
-  this->execute_callback_();
-}
-
-std::shared_ptr<void>
-ExecutorNotifyWaitable::take_data()
-{
-  return nullptr;
-}
-
-std::shared_ptr<void>
-ExecutorNotifyWaitable::take_data_by_entity_id(size_t id)
-{
-  (void) id;
-  return nullptr;
-}
-
-void
-ExecutorNotifyWaitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
-{
-  // The second argument of the callback could be used to identify which guard condition
-  // triggered the event.
-  // We could indicate which of the guard conditions was triggered, but the executor
-  // is already going to check that.
-  auto gc_callback = [callback](size_t count) {
-      callback(count, 0);
-    };
-
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-
-  on_ready_callback_ = gc_callback;
-  for (auto weak_gc : notify_guard_conditions_) {
-    auto gc = weak_gc.lock();
-    if (!gc) {
-      continue;
-    }
-    gc->set_on_trigger_callback(on_ready_callback_);
-  }
-}
-
-RCLCPP_PUBLIC
-void
-ExecutorNotifyWaitable::clear_on_ready_callback()
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-
-  on_ready_callback_ = nullptr;
-  for (auto weak_gc : notify_guard_conditions_) {
-    auto gc = weak_gc.lock();
-    if (!gc) {
-      continue;
-    }
-    gc->set_on_trigger_callback(nullptr);
-  }
-}
-
-void
-ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  auto guard_condition = weak_guard_condition.lock();
-  if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) {
-    notify_guard_conditions_.insert(weak_guard_condition);
-    if (on_ready_callback_) {
-      guard_condition->set_on_trigger_callback(on_ready_callback_);
-    }
-  }
-}
-
-void
-ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  if (notify_guard_conditions_.count(weak_guard_condition) != 0) {
-    notify_guard_conditions_.erase(weak_guard_condition);
-    auto guard_condition = weak_guard_condition.lock();
-    // If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset
-    if (guard_condition && on_ready_callback_) {
-      guard_condition->set_on_trigger_callback(nullptr);
-    }
-  }
-}
-
-size_t
-ExecutorNotifyWaitable::get_number_of_ready_guard_conditions()
-{
-  std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  return notify_guard_conditions_.size();
-}
-
-}  // namespace executors
-}  // namespace rclcpp

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -99,19 +99,6 @@ MultiThreadedExecutor::run(size_t this_thread_number)
 
     execute_any_executable(any_exec);
 
-    if (any_exec.callback_group &&
-      any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive &&
-      any_exec.callback_group->size() > 1)
-    {
-      try {
-        interrupt_guard_condition_->trigger();
-      } catch (const rclcpp::exceptions::RCLError & ex) {
-        throw std::runtime_error(
-                std::string(
-                  "Failed to trigger guard condition on callback group change: ") + ex.what());
-      }
-    }
-
     // Clear the callback_group to prevent the AnyExecutable destructor from
     // resetting the callback group `can_be_taken_from`
     any_exec.callback_group.reset();

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -0,0 +1,524 @@
+// 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/static_executor_entities_collector.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#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;
+
+StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
+{
+  // Disassociate all callback groups and thus nodes.
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  // Disassociate all nodes
+  for (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+      has_executor.store(false);
+    }
+  }
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  exec_list_.clear();
+  weak_nodes_.clear();
+  weak_nodes_to_guard_conditions_.clear();
+}
+
+void
+StaticExecutorEntitiesCollector::init(
+  rcl_wait_set_t * p_wait_set,
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
+{
+  // Empty initialize executable list
+  exec_list_ = rclcpp::experimental::ExecutableList();
+  // Get executor's wait_set_ pointer
+  p_wait_set_ = p_wait_set;
+  // Get executor's memory strategy ptr
+  if (memory_strategy == nullptr) {
+    throw std::runtime_error("Received NULL memory strategy in executor waitable.");
+  }
+  memory_strategy_ = memory_strategy;
+
+  // Get memory strategy and executable list. Prepare wait_set_
+  std::shared_ptr<void> shared_ptr;
+  execute(shared_ptr);
+
+  // The entities collector is now initialized
+  initialized_ = true;
+}
+
+void
+StaticExecutorEntitiesCollector::fini()
+{
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
+std::shared_ptr<void>
+StaticExecutorEntitiesCollector::take_data()
+{
+  return nullptr;
+}
+
+void
+StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
+{
+  (void) data;
+  // Fill memory strategy with entities coming from weak_nodes_
+  fill_memory_strategy();
+  // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy)
+  fill_executable_list();
+  // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize)
+  prepare_wait_set();
+  // Add new nodes guard conditions to map
+  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+  for (const auto & weak_node : new_nodes_) {
+    if (auto node_ptr = weak_node.lock()) {
+      const auto & gc = node_ptr->get_notify_guard_condition();
+      weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+    }
+  }
+  new_nodes_.clear();
+}
+
+void
+StaticExecutorEntitiesCollector::fill_memory_strategy()
+{
+  memory_strategy_->clear_handles();
+  bool has_invalid_weak_groups_or_nodes =
+    memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_);
+  // Clean up any invalid nodes, if they were detected
+  if (has_invalid_weak_groups_or_nodes) {
+    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+    for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+      auto & weak_group_ptr = pair.first;
+      auto & weak_node_ptr = pair.second;
+      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+        invalid_group_ptrs.push_back(weak_group_ptr);
+      }
+    }
+    std::for_each(
+      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+        weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
+      });
+  }
+  has_invalid_weak_groups_or_nodes =
+    memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_);
+  // Clean up any invalid nodes, if they were detected
+  if (has_invalid_weak_groups_or_nodes) {
+    std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
+    for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+      auto & weak_group_ptr = pair.first;
+      const auto & weak_node_ptr = pair.second;
+      if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
+        invalid_group_ptrs.push_back(weak_group_ptr);
+      }
+    }
+    std::for_each(
+      invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
+      [this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
+        weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
+      });
+  }
+
+  // Add the static executor waitable to the memory strategy
+  memory_strategy_->add_waitable_handle(this->shared_from_this());
+}
+
+void
+StaticExecutorEntitiesCollector::fill_executable_list()
+{
+  exec_list_.clear();
+  add_callback_groups_from_nodes_associated_to_executor();
+  fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_);
+  fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_);
+  // 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 &
+  weak_groups_to_nodes)
+{
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!node || !group || !group->can_be_taken_from().load()) {
+      continue;
+    }
+    group->find_timer_ptrs_if(
+      [this](const rclcpp::TimerBase::SharedPtr & timer) {
+        if (timer) {
+          exec_list_.add_timer(timer);
+        }
+        return false;
+      });
+    group->find_subscription_ptrs_if(
+      [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+        if (subscription) {
+          exec_list_.add_subscription(subscription);
+        }
+        return false;
+      });
+    group->find_service_ptrs_if(
+      [this](const rclcpp::ServiceBase::SharedPtr & service) {
+        if (service) {
+          exec_list_.add_service(service);
+        }
+        return false;
+      });
+    group->find_client_ptrs_if(
+      [this](const rclcpp::ClientBase::SharedPtr & client) {
+        if (client) {
+          exec_list_.add_client(client);
+        }
+        return false;
+      });
+    group->find_waitable_ptrs_if(
+      [this](const rclcpp::Waitable::SharedPtr & waitable) {
+        if (waitable) {
+          exec_list_.add_waitable(waitable);
+        }
+        return false;
+      });
+  }
+}
+
+void
+StaticExecutorEntitiesCollector::prepare_wait_set()
+{
+  // clear wait set
+  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
+  }
+
+  // The size of waitables are accounted for in size of the other entities
+  rcl_ret_t ret = rcl_wait_set_resize(
+    p_wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+    memory_strategy_->number_of_ready_events());
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error(
+            std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str);
+  }
+}
+
+void
+StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
+{
+  // clear wait set (memset to '0' all wait_set_ entities
+  // but keeps the wait_set_ number of entities)
+  if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
+  }
+
+  if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) {
+    throw std::runtime_error("Couldn't fill wait set");
+  }
+
+  rcl_ret_t status =
+    rcl_wait(p_wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+
+  if (status == RCL_RET_WAIT_SET_EMPTY) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in rcl_wait(). This should never happen.");
+  } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(status, "rcl_wait() failed");
+  }
+}
+
+void
+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);
+  }
+}
+
+size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
+{
+  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+  return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
+}
+
+bool
+StaticExecutorEntitiesCollector::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
+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)
+{
+  // 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;
+}
+
+bool
+StaticExecutorEntitiesCollector::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
+StaticExecutorEntitiesCollector::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
+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;
+}
+
+bool
+StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
+{
+  // Check wait_set guard_conditions for added/removed entities to/from a node
+  for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
+    if (p_wait_set->guard_conditions[i] != NULL) {
+      auto found_guard_condition = std::find_if(
+        weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(),
+        [&](std::pair<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+        const GuardCondition *> pair) -> bool {
+          const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition();
+          return &rcl_gc == p_wait_set->guard_conditions[i];
+        });
+      if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) {
+        return true;
+      }
+    }
+  }
+  // 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

@@ -12,21 +12,31 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+
+#include <chrono>
+#include <memory>
+#include <utility>
+#include <vector>
+
 #include "rcpputils/scope_exit.hpp"
 
-#include "rclcpp/executors/static_single_threaded_executor.hpp"
-#include "rclcpp/any_executable.hpp"
-
 using rclcpp::executors::StaticSingleThreadedExecutor;
+using rclcpp::experimental::ExecutableList;
 
-StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options)
+StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(
+  const rclcpp::ExecutorOptions & options)
 : rclcpp::Executor(options)
 {
+  entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
 }
 
-StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
+StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor()
+{
+  if (entities_collector_->is_init()) {
+    entities_collector_->fini();
+  }
+}
 
 void
 StaticSingleThreadedExecutor::spin()
@@ -36,25 +46,14 @@ StaticSingleThreadedExecutor::spin()
   }
   RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
-  // This is essentially the contents of the rclcpp::Executor::wait_for_work method,
-  // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
-  // behavior.
+  // Set memory_strategy_ and exec_list_ based on weak_nodes_
+  // Prepare wait_set_ based on memory_strategy_
+  entities_collector_->init(&wait_set_, memory_strategy_);
+
   while (rclcpp::ok(this->context_) && spinning.load()) {
-    std::deque<rclcpp::AnyExecutable> to_exec;
-
-    std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
-    }
-
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      continue;
-    }
-    execute_ready_executables(current_collection_, wait_result, false);
+    // Refresh wait set and wait for work
+    entities_collector_->refresh_wait_set();
+    execute_ready_executables();
   }
 }
 
@@ -81,6 +80,11 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
 void
 StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
 {
+  // Make sure the entities collector has been initialized
+  if (!entities_collector_->is_init()) {
+    entities_collector_->init(&wait_set_, memory_strategy_);
+  }
+
   auto start = std::chrono::steady_clock::now();
   auto max_duration_not_elapsed = [max_duration, start]() {
       if (std::chrono::nanoseconds(0) == max_duration) {
@@ -101,21 +105,9 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
-    std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
-    }
-
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      continue;
-    }
-
+    entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero());
     // Execute ready executables
-    bool work_available = execute_ready_executables(current_collection_, wait_result, false);
+    bool work_available = execute_ready_executables();
     if (!work_available || !exhaustive) {
       break;
     }
@@ -125,122 +117,164 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 void
 StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
+  // Make sure the entities collector has been initialized
+  if (!entities_collector_->is_init()) {
+    entities_collector_->init(&wait_set_, memory_strategy_);
+  }
+
   if (rclcpp::ok(context_) && spinning.load()) {
-    std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
-      this->collect_entities();
-    }
-
-    auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
-    if (wait_result.kind() == WaitResultKind::Empty) {
-      RCUTILS_LOG_WARN_NAMED(
-        "rclcpp",
-        "empty wait set received in wait(). This should never happen.");
-      return;
-    }
-
+    // Wait until we have a ready entity or timeout expired
+    entities_collector_->refresh_wait_set(timeout);
     // Execute ready executables
-    execute_ready_executables(current_collection_, wait_result, true);
+    execute_ready_executables(true);
   }
 }
 
-// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor
-// from the original implementation.
-bool StaticSingleThreadedExecutor::execute_ready_executables(
-  const rclcpp::executors::ExecutorEntitiesCollection & collection,
-  rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
-  bool spin_once)
+void
+StaticSingleThreadedExecutor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
+{
+  bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
+  if (is_new_node && notify) {
+    // Interrupt waiting to handle new node
+    interrupt_guard_condition_.trigger();
+  }
+}
+
+void
+StaticSingleThreadedExecutor::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  bool is_new_node = entities_collector_->add_node(node_ptr);
+  if (is_new_node && notify) {
+    // Interrupt waiting to handle new node
+    interrupt_guard_condition_.trigger();
+  }
+}
+
+void
+StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->add_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+StaticSingleThreadedExecutor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
+{
+  bool node_removed = entities_collector_->remove_callback_group(group_ptr);
+  // If the node was matched and removed, interrupt waiting
+  if (node_removed && notify) {
+    interrupt_guard_condition_.trigger();
+  }
+}
+
+void
+StaticSingleThreadedExecutor::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  bool node_removed = entities_collector_->remove_node(node_ptr);
+  if (!node_removed) {
+    throw std::runtime_error("Node needs to be associated with this executor.");
+  }
+  // If the node was matched and removed, interrupt waiting
+  if (notify) {
+    interrupt_guard_condition_.trigger();
+  }
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_all_callback_groups()
+{
+  return entities_collector_->get_all_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_manually_added_callback_groups()
+{
+  return entities_collector_->get_manually_added_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
+{
+  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
+}
+
+void
+StaticSingleThreadedExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->remove_node(node_ptr->get_node_base_interface(), notify);
+}
+
+bool
+StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once)
 {
   bool any_ready_executable = false;
 
-  if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
-    return any_ready_executable;
+  // Execute all the ready subscriptions
+  for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) {
+    if (i < entities_collector_->get_number_of_subscriptions()) {
+      if (wait_set_.subscriptions[i]) {
+        execute_subscription(entities_collector_->get_subscription(i));
+        if (spin_once) {
+          return true;
+        }
+        any_ready_executable = true;
+      }
+    }
   }
-
-  auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
-    if (nullptr == rcl_wait_set.timers[ii]) {continue;}
-    auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
-    if (entity_iter != collection.timers.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
+  // Execute all the ready timers
+  for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
+    if (i < entities_collector_->get_number_of_timers()) {
+      if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
+        auto timer = entities_collector_->get_timer(i);
+        timer->call();
+        execute_timer(std::move(timer));
+        if (spin_once) {
+          return true;
+        }
+        any_ready_executable = true;
       }
-      if (!entity->call()) {
-        continue;
+    }
+  }
+  // Execute all the ready services
+  for (size_t i = 0; i < wait_set_.size_of_services; ++i) {
+    if (i < entities_collector_->get_number_of_services()) {
+      if (wait_set_.services[i]) {
+        execute_service(entities_collector_->get_service(i));
+        if (spin_once) {
+          return true;
+        }
+        any_ready_executable = true;
       }
-      execute_timer(entity);
+    }
+  }
+  // Execute all the ready clients
+  for (size_t i = 0; i < wait_set_.size_of_clients; ++i) {
+    if (i < entities_collector_->get_number_of_clients()) {
+      if (wait_set_.clients[i]) {
+        execute_client(entities_collector_->get_client(i));
+        if (spin_once) {
+          return true;
+        }
+        any_ready_executable = true;
+      }
+    }
+  }
+  // Execute all the ready waitables
+  for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
+    auto waitable = entities_collector_->get_waitable(i);
+    if (waitable->is_ready(&wait_set_)) {
+      auto data = waitable->take_data();
+      waitable->execute(data);
       if (spin_once) {
         return true;
       }
       any_ready_executable = true;
     }
   }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
-    if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
-    auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
-    if (entity_iter != collection.subscriptions.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      execute_subscription(entity);
-      if (spin_once) {
-        return true;
-      }
-      any_ready_executable = true;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
-    if (nullptr == rcl_wait_set.services[ii]) {continue;}
-    auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
-    if (entity_iter != collection.services.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      execute_service(entity);
-      if (spin_once) {
-        return true;
-      }
-      any_ready_executable = true;
-    }
-  }
-
-  for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
-    if (nullptr == rcl_wait_set.clients[ii]) {continue;}
-    auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
-    if (entity_iter != collection.clients.end()) {
-      auto entity = entity_iter->second.entity.lock();
-      if (!entity) {
-        continue;
-      }
-      execute_client(entity);
-      if (spin_once) {
-        return true;
-      }
-      any_ready_executable = true;
-    }
-  }
-
-  for (auto & [handle, entry] : collection.waitables) {
-    auto waitable = entry.entity.lock();
-    if (!waitable) {
-      continue;
-    }
-    if (!waitable->is_ready(&rcl_wait_set)) {
-      continue;
-    }
-
-    auto data = waitable->take_data();
-    waitable->execute(data);
-    if (spin_once) {
-      return true;
-    }
-    any_ready_executable = true;
-  }
   return any_ready_executable;
 }

rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp

@@ -15,11 +15,15 @@
 #include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
 
 #include <memory>
+#include <queue>
+#include <string>
 #include <utility>
 #include <vector>
 
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/exceptions/exceptions.hpp"
+
 using namespace std::chrono_literals;
 
 using rclcpp::experimental::executors::EventsExecutor;
@@ -37,12 +41,9 @@ EventsExecutor::EventsExecutor(
   events_queue_ = std::move(events_queue);
 
   // Create timers manager
-  // The timers manager can be used either to only track timers (in this case an expired
-  // timer will generate an executor event and then it will be executed by the executor thread)
-  // or it can also take care of executing expired timers in its dedicated thread.
-  std::function<void(const rclcpp::TimerBase *)> timer_on_ready_cb = nullptr;
-  if (!execute_timers_separate_thread) {
-    timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) {
+  std::function<void(void *)> timer_on_ready_cb = nullptr;
+  if (execute_timers_separate_thread) {
+    timer_on_ready_cb = [this](const void * timer_id) {
         ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
         this->events_queue_->enqueue(event);
       };
@@ -50,53 +51,22 @@ EventsExecutor::EventsExecutor(
   timers_manager_ =
     std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
 
-  notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
-    [this]() {
-      // This callback is invoked when:
-      // - the interrupt or shutdown guard condition is triggered:
-      //    ---> we need to wake up the executor so that it can terminate
-      // - a node or callback group guard condition is triggered:
-      //    ---> the entities collection is changed, we need to update callbacks
-      notify_waitable_event_pushed_ = false;
-      this->refresh_current_collection_from_callback_groups();
-    });
+  // Create entities collector
+  entities_collector_ = std::make_shared<EventsExecutorEntitiesCollector>(this);
+  entities_collector_->init();
 
-  notify_waitable_->add_guard_condition(interrupt_guard_condition_);
-  notify_waitable_->add_guard_condition(shutdown_guard_condition_);
+  // Setup the executor notifier to wake up the executor when some guard conditions are tiggered.
+  // The added guard conditions are guaranteed to not go out of scope before the executor itself.
+  executor_notifier_ = std::make_shared<EventsExecutorNotifyWaitable>();
+  executor_notifier_->add_guard_condition(shutdown_guard_condition_.get());
+  executor_notifier_->add_guard_condition(&interrupt_guard_condition_);
 
-  notify_waitable_->set_on_ready_callback(
-    this->create_waitable_callback(notify_waitable_.get()));
-
-  auto notify_waitable_entity_id = notify_waitable_.get();
-  notify_waitable_->set_on_ready_callback(
-    [this, notify_waitable_entity_id](size_t num_events, int waitable_data) {
-      // The notify waitable has a special callback.
-      // We don't care about how many events as when we wake up the executor we are going to
-      // process everything regardless.
-      // For the same reason, if an event of this type has already been pushed but it has not been
-      // processed yet, we avoid pushing additional events.
-      (void)num_events;
-      if (notify_waitable_event_pushed_.exchange(true)) {
-        return;
-      }
-
-      ExecutorEvent event =
-      {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
-      this->events_queue_->enqueue(event);
-    });
-
-  this->entities_collector_ =
-    std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(notify_waitable_);
-
-  this->current_entities_collection_ =
-    std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
+  entities_collector_->add_waitable(executor_notifier_);
 }
 
 EventsExecutor::~EventsExecutor()
 {
   spinning.store(false);
-  notify_waitable_->clear_on_ready_callback();
-  this->refresh_current_collection({});
 }
 
 void
@@ -230,9 +200,7 @@ EventsExecutor::add_node(
   (void) notify;
 
   // Add node to entities collector
-  this->entities_collector_->add_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
+  entities_collector_->add_node(node_ptr);
 }
 
 void
@@ -252,9 +220,7 @@ EventsExecutor::remove_node(
   // This will result in un-setting all the event callbacks from its entities.
   // After this function returns, this executor will not receive any more events associated
   // to these entities.
-  this->entities_collector_->remove_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
+  entities_collector_->remove_node(node_ptr);
 }
 
 void
@@ -269,23 +235,19 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
   switch (event.type) {
     case ExecutorEventType::CLIENT_EVENT:
       {
-        auto client = this->retrieve_entity(
-          static_cast<const rcl_client_t *>(event.entity_key),
-          current_entities_collection_->clients);
+        auto client = entities_collector_->get_client(event.exec_entity_id);
 
         if (client) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_client(client);
           }
         }
-
         break;
       }
     case ExecutorEventType::SUBSCRIPTION_EVENT:
       {
-        auto subscription = this->retrieve_entity(
-          static_cast<const rcl_subscription_t *>(event.entity_key),
-          current_entities_collection_->subscriptions);
+        auto subscription = entities_collector_->get_subscription(event.exec_entity_id);
+
         if (subscription) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_subscription(subscription);
@@ -295,32 +257,27 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       }
     case ExecutorEventType::SERVICE_EVENT:
       {
-        auto service = this->retrieve_entity(
-          static_cast<const rcl_service_t *>(event.entity_key),
-          current_entities_collection_->services);
+        auto service = entities_collector_->get_service(event.exec_entity_id);
 
         if (service) {
           for (size_t i = 0; i < event.num_events; i++) {
             execute_service(service);
           }
         }
-
         break;
       }
     case ExecutorEventType::TIMER_EVENT:
       {
-        timers_manager_->execute_ready_timer(
-          static_cast<const rclcpp::TimerBase *>(event.entity_key));
+        timers_manager_->execute_ready_timer(event.exec_entity_id);
         break;
       }
     case ExecutorEventType::WAITABLE_EVENT:
       {
-        auto waitable = this->retrieve_entity(
-          static_cast<const rclcpp::Waitable *>(event.entity_key),
-          current_entities_collection_->waitables);
+        auto waitable = entities_collector_->get_waitable(event.exec_entity_id);
+
         if (waitable) {
           for (size_t i = 0; i < event.num_events; i++) {
-            auto data = waitable->take_data_by_entity_id(event.waitable_data);
+            auto data = waitable->take_data_by_entity_id(event.gen_entity_id);
             waitable->execute(data);
           }
         }
@@ -338,11 +295,7 @@ EventsExecutor::add_callback_group(
   // This field is unused because we don't have to wake up
   // the executor when a callback group is added.
   (void)notify;
-  (void)node_ptr;
-
-  this->entities_collector_->add_callback_group(group_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
+  entities_collector_->add_callback_group(group_ptr, node_ptr);
 }
 
 void
@@ -352,137 +305,23 @@ EventsExecutor::remove_callback_group(
   // This field is unused because we don't have to wake up
   // the executor when a callback group is removed.
   (void)notify;
-
-  this->entities_collector_->remove_callback_group(group_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
+  entities_collector_->remove_callback_group(group_ptr);
 }
 
 std::vector<rclcpp::CallbackGroup::WeakPtr>
 EventsExecutor::get_all_callback_groups()
 {
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_all_callback_groups();
+  return entities_collector_->get_all_callback_groups();
 }
 
 std::vector<rclcpp::CallbackGroup::WeakPtr>
 EventsExecutor::get_manually_added_callback_groups()
 {
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_manually_added_callback_groups();
+  return entities_collector_->get_manually_added_callback_groups();
 }
 
 std::vector<rclcpp::CallbackGroup::WeakPtr>
 EventsExecutor::get_automatically_added_callback_groups_from_nodes()
 {
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_automatically_added_callback_groups();
-}
-
-void
-EventsExecutor::refresh_current_collection_from_callback_groups()
-{
-  this->entities_collector_->update_collections();
-  auto callback_groups = this->entities_collector_->get_all_callback_groups();
-  rclcpp::executors::ExecutorEntitiesCollection new_collection;
-  rclcpp::executors::build_entities_collection(callback_groups, new_collection);
-
-  // TODO(alsora): this may be implemented in a better way.
-  // We need the notify waitable to be included in the executor "current_collection"
-  // because we need to be able to retrieve events for it.
-  // We could explicitly check for the notify waitable ID when we receive a waitable event
-  // but I think that it's better if the waitable was in the collection and it could be
-  // retrieved in the "standard" way.
-  // To do it, we need to add the notify waitable as an entry in both the new and
-  // current collections such that it's neither added or removed.
-  rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
-  new_collection.waitables.insert(
-  {
-    this->notify_waitable_.get(),
-    {this->notify_waitable_, weak_group_ptr}
-  });
-
-  this->current_entities_collection_->waitables.insert(
-  {
-    this->notify_waitable_.get(),
-    {this->notify_waitable_, weak_group_ptr}
-  });
-
-  this->refresh_current_collection(new_collection);
-}
-
-void
-EventsExecutor::refresh_current_collection(
-  const rclcpp::executors::ExecutorEntitiesCollection & new_collection)
-{
-  current_entities_collection_->timers.update(
-    new_collection.timers,
-    [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);},
-    [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);});
-
-  current_entities_collection_->subscriptions.update(
-    new_collection.subscriptions,
-    [this](auto subscription) {
-      subscription->set_on_new_message_callback(
-        this->create_entity_callback(
-          subscription->get_subscription_handle().get(), ExecutorEventType::SUBSCRIPTION_EVENT));
-    },
-    [](auto subscription) {subscription->clear_on_new_message_callback();});
-
-  current_entities_collection_->clients.update(
-    new_collection.clients,
-    [this](auto client) {
-      client->set_on_new_response_callback(
-        this->create_entity_callback(
-          client->get_client_handle().get(), ExecutorEventType::CLIENT_EVENT));
-    },
-    [](auto client) {client->clear_on_new_response_callback();});
-
-  current_entities_collection_->services.update(
-    new_collection.services,
-    [this](auto service) {
-      service->set_on_new_request_callback(
-        this->create_entity_callback(
-          service->get_service_handle().get(), ExecutorEventType::SERVICE_EVENT));
-    },
-    [](auto service) {service->clear_on_new_request_callback();});
-
-  // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS
-  /*
-  current_entities_collection_->guard_conditions.update(new_collection.guard_conditions,
-    [](auto guard_condition) {(void)guard_condition;},
-    [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);});
-  */
-
-  current_entities_collection_->waitables.update(
-    new_collection.waitables,
-    [this](auto waitable) {
-      waitable->set_on_ready_callback(
-        this->create_waitable_callback(waitable.get()));
-    },
-    [](auto waitable) {waitable->clear_on_ready_callback();});
-}
-
-std::function<void(size_t)>
-EventsExecutor::create_entity_callback(
-  void * entity_key, ExecutorEventType event_type)
-{
-  std::function<void(size_t)>
-  callback = [this, entity_key, event_type](size_t num_events) {
-      ExecutorEvent event = {entity_key, -1, event_type, num_events};
-      this->events_queue_->enqueue(event);
-    };
-  return callback;
-}
-
-std::function<void(size_t, int)>
-EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key)
-{
-  std::function<void(size_t, int)>
-  callback = [this, entity_key](size_t num_events, int waitable_data) {
-      ExecutorEvent event =
-      {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
-      this->events_queue_->enqueue(event);
-    };
-  return callback;
+  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
 }

rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor_entities_collector.cpp

@@ -0,0 +1,699 @@
+// Copyright 2023 iRobot Corporation.
+//
+// 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/experimental/executors/events_executor/events_executor_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"
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
+
+using rclcpp::experimental::executors::ExecutorEvent;
+using rclcpp::experimental::executors::ExecutorEventType;
+using rclcpp::experimental::executors::EventsExecutorEntitiesCollector;
+
+EventsExecutorEntitiesCollector::EventsExecutorEntitiesCollector(
+  rclcpp::experimental::executors::EventsExecutor * executor)
+{
+  if (executor == nullptr) {
+    throw std::runtime_error("Received nullptr executor in EventsExecutorEntitiesCollector.");
+  }
+
+  associated_executor_ = executor;
+  timers_manager_ = associated_executor_->timers_manager_;
+}
+
+void
+EventsExecutorEntitiesCollector::init()
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // Add the EventsExecutorEntitiesCollector shared_ptr to waitables map
+  weak_waitables_map_.emplace(this, this->shared_from_this());
+}
+
+EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector()
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  // Disassociate all callback groups and thus nodes.
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    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_) {
+    auto group = pair.first.lock();
+    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
+  for (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+      has_executor.store(false);
+      node_removed_impl(node);
+    }
+  }
+
+  // Unset callback group notify guard condition executor callback
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
+    auto group = pair.first.lock();
+    if (group) {
+      auto & group_gc = pair.second;
+      unset_guard_condition_callback(group_gc);
+    }
+  }
+
+  weak_clients_map_.clear();
+  weak_services_map_.clear();
+  weak_waitables_map_.clear();
+  weak_subscriptions_map_.clear();
+  weak_nodes_to_guard_conditions_.clear();
+  weak_groups_to_guard_conditions_.clear();
+
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_nodes_.clear();
+}
+
+void
+EventsExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
+{
+  // This function is called when the associated executor is notified that something changed.
+  // We do not know if an entity has been added or removed so we have to rebuild everything.
+  (void)data;
+
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  timers_manager_->clear();
+
+  // If a registered node has a new callback group, register the group.
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  // For all groups registered in the executor, set their event callbacks.
+  set_entities_event_callbacks_from_map(weak_groups_associated_with_executor_to_nodes_);
+  set_entities_event_callbacks_from_map(weak_groups_to_nodes_associated_with_executor_);
+}
+
+std::shared_ptr<void>
+EventsExecutorEntitiesCollector::take_data()
+{
+  return nullptr;
+}
+
+std::shared_ptr<void>
+EventsExecutorEntitiesCollector::take_data_by_entity_id(size_t id)
+{
+  (void)id;
+  return nullptr;
+}
+
+void
+EventsExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  (void)wait_set;
+}
+
+bool
+EventsExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
+{
+  (void)p_wait_set;
+  return false;
+}
+
+bool
+EventsExecutorEntitiesCollector::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
+EventsExecutorEntitiesCollector::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.");
+  }
+
+  if (is_new_node) {
+    node_added_impl(node_ptr);
+  }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+
+    rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
+  }
+
+  callback_group_added_impl(group_ptr);
+
+  return is_new_node;
+}
+
+bool
+EventsExecutorEntitiesCollector::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
+EventsExecutorEntitiesCollector::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
+EventsExecutorEntitiesCollector::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.
+  bool node_removed = false;
+  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);
+    node_removed = true;
+  }
+
+  weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
+  return node_removed;
+}
+
+bool
+EventsExecutorEntitiesCollector::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
+EventsExecutorEntitiesCollector::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
+EventsExecutorEntitiesCollector::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>
+EventsExecutorEntitiesCollector::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>
+EventsExecutorEntitiesCollector::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>
+EventsExecutorEntitiesCollector::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;
+}
+
+void
+EventsExecutorEntitiesCollector::callback_group_added_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group;
+
+  auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+  if (iter != weak_groups_to_guard_conditions_.end()) {
+    // Set an event callback for the group's notify guard condition, so if new entities are added
+    // or removed to this node we will receive an event.
+    set_guard_condition_callback(iter->second);
+  }
+  // For all entities in the callback group, set their event callback
+  set_callback_group_entities_callbacks(group);
+}
+
+void
+EventsExecutorEntitiesCollector::node_added_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto notify_guard_condition = &(node->get_notify_guard_condition());
+  // Set an event callback for the node's notify guard condition, so if new entities are added
+  // or removed to this node we will receive an event.
+  set_guard_condition_callback(notify_guard_condition);
+
+  // Store node's notify guard condition
+  weak_nodes_to_guard_conditions_[node] = notify_guard_condition;
+}
+
+void
+EventsExecutorEntitiesCollector::callback_group_removed_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // For all the entities in the group, unset their callbacks
+  unset_callback_group_entities_callbacks(group);
+}
+
+void
+EventsExecutorEntitiesCollector::node_removed_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // Node doesn't have more callback groups associated to the executor.
+  // Unset the event callback for the node's notify guard condition, to stop
+  // receiving events if entities are added or removed to this node.
+  unset_guard_condition_callback(&(node->get_notify_guard_condition()));
+
+  // Remove guard condition from list
+  rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr(node);
+  weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+}
+
+void
+EventsExecutorEntitiesCollector::set_entities_event_callbacks_from_map(
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!node || !group || !group->can_be_taken_from().load()) {
+      continue;
+    }
+    set_callback_group_entities_callbacks(group);
+  }
+}
+
+void
+EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  // Timers are handled by the timers manager
+  group->find_timer_ptrs_if(
+    [this](const rclcpp::TimerBase::SharedPtr & timer) {
+      if (timer) {
+        timers_manager_->add_timer(timer);
+      }
+      return false;
+    });
+
+  // Set callbacks for all other entity types
+  group->find_subscription_ptrs_if(
+    [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+      if (subscription) {
+        weak_subscriptions_map_.emplace(subscription.get(), subscription);
+
+        subscription->set_on_new_message_callback(
+          create_entity_callback(subscription.get(), ExecutorEventType::SUBSCRIPTION_EVENT));
+      }
+      return false;
+    });
+  group->find_service_ptrs_if(
+    [this](const rclcpp::ServiceBase::SharedPtr & service) {
+      if (service) {
+        weak_services_map_.emplace(service.get(), service);
+
+        service->set_on_new_request_callback(
+          create_entity_callback(service.get(), ExecutorEventType::SERVICE_EVENT));
+      }
+      return false;
+    });
+  group->find_client_ptrs_if(
+    [this](const rclcpp::ClientBase::SharedPtr & client) {
+      if (client) {
+        weak_clients_map_.emplace(client.get(), client);
+
+        client->set_on_new_response_callback(
+          create_entity_callback(client.get(), ExecutorEventType::CLIENT_EVENT));
+      }
+      return false;
+    });
+  group->find_waitable_ptrs_if(
+    [this](const rclcpp::Waitable::SharedPtr & waitable) {
+      if (waitable) {
+        weak_waitables_map_.emplace(waitable.get(), waitable);
+
+        waitable->set_on_ready_callback(
+          create_waitable_callback(waitable.get()));
+      }
+      return false;
+    });
+}
+
+void
+EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  auto iter = weak_groups_to_guard_conditions_.find(group);
+
+  if (iter != weak_groups_to_guard_conditions_.end()) {
+    unset_guard_condition_callback(iter->second);
+  }
+
+  // Timers are handled by the timers manager
+  group->find_timer_ptrs_if(
+    [this](const rclcpp::TimerBase::SharedPtr & timer) {
+      if (timer) {
+        timers_manager_->remove_timer(timer);
+      }
+      return false;
+    });
+
+  // Unset callbacks for all other entity types
+  group->find_subscription_ptrs_if(
+    [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+      if (subscription) {
+        subscription->clear_on_new_message_callback();
+        weak_subscriptions_map_.erase(subscription.get());
+      }
+      return false;
+    });
+  group->find_service_ptrs_if(
+    [this](const rclcpp::ServiceBase::SharedPtr & service) {
+      if (service) {
+        service->clear_on_new_request_callback();
+        weak_services_map_.erase(service.get());
+      }
+      return false;
+    });
+  group->find_client_ptrs_if(
+    [this](const rclcpp::ClientBase::SharedPtr & client) {
+      if (client) {
+        client->clear_on_new_response_callback();
+        weak_clients_map_.erase(client.get());
+      }
+      return false;
+    });
+  group->find_waitable_ptrs_if(
+    [this](const rclcpp::Waitable::SharedPtr & waitable) {
+      if (waitable) {
+        waitable->clear_on_ready_callback();
+        weak_waitables_map_.erase(waitable.get());
+      }
+      return false;
+    });
+}
+
+void
+EventsExecutorEntitiesCollector::set_guard_condition_callback(
+  rclcpp::GuardCondition * guard_condition)
+{
+  auto gc_callback = [this](size_t num_events) {
+      // Override num events (we don't care more than a single event)
+      num_events = 1;
+      int gc_id = -1;
+      ExecutorEvent event = {this, gc_id, ExecutorEventType::WAITABLE_EVENT, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+
+  guard_condition->set_on_trigger_callback(gc_callback);
+}
+
+void
+EventsExecutorEntitiesCollector::unset_guard_condition_callback(
+  rclcpp::GuardCondition * guard_condition)
+{
+  guard_condition->set_on_trigger_callback(nullptr);
+}
+
+rclcpp::SubscriptionBase::SharedPtr
+EventsExecutorEntitiesCollector::get_subscription(const void * subscription_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_subscriptions_map_.find(subscription_id);
+
+  if (it != weak_subscriptions_map_.end()) {
+    auto subscription_weak_ptr = it->second;
+    auto subscription_shared_ptr = subscription_weak_ptr.lock();
+
+    if (subscription_shared_ptr) {
+      return subscription_shared_ptr;
+    }
+
+    // The subscription expired, remove from map
+    weak_subscriptions_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::ClientBase::SharedPtr
+EventsExecutorEntitiesCollector::get_client(const void * client_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_clients_map_.find(client_id);
+
+  if (it != weak_clients_map_.end()) {
+    auto client_weak_ptr = it->second;
+    auto client_shared_ptr = client_weak_ptr.lock();
+
+    if (client_shared_ptr) {
+      return client_shared_ptr;
+    }
+
+    // The client expired, remove from map
+    weak_clients_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::ServiceBase::SharedPtr
+EventsExecutorEntitiesCollector::get_service(const void * service_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_services_map_.find(service_id);
+
+  if (it != weak_services_map_.end()) {
+    auto service_weak_ptr = it->second;
+    auto service_shared_ptr = service_weak_ptr.lock();
+
+    if (service_shared_ptr) {
+      return service_shared_ptr;
+    }
+
+    // The service expired, remove from map
+    weak_services_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::Waitable::SharedPtr
+EventsExecutorEntitiesCollector::get_waitable(const void * waitable_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_waitables_map_.find(waitable_id);
+
+  if (it != weak_waitables_map_.end()) {
+    auto waitable_weak_ptr = it->second;
+    auto waitable_shared_ptr = waitable_weak_ptr.lock();
+
+    if (waitable_shared_ptr) {
+      return waitable_shared_ptr;
+    }
+
+    // The waitable expired, remove from map
+    weak_waitables_map_.erase(it);
+  }
+  return nullptr;
+}
+
+void
+EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitable)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  weak_waitables_map_.emplace(waitable.get(), waitable);
+
+  waitable->set_on_ready_callback(
+    create_waitable_callback(waitable.get()));
+}
+
+std::function<void(size_t)>
+EventsExecutorEntitiesCollector::create_entity_callback(
+  void * exec_entity_id, ExecutorEventType event_type)
+{
+  std::function<void(size_t)>
+  callback = [this, exec_entity_id, event_type](size_t num_events) {
+      ExecutorEvent event = {exec_entity_id, -1, event_type, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+  return callback;
+}
+
+std::function<void(size_t, int)>
+EventsExecutorEntitiesCollector::create_waitable_callback(void * exec_entity_id)
+{
+  std::function<void(size_t, int)>
+  callback = [this, exec_entity_id](size_t num_events, int gen_entity_id) {
+      ExecutorEvent event =
+      {exec_entity_id, gen_entity_id, ExecutorEventType::WAITABLE_EVENT, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+  return callback;
+}

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -21,13 +21,11 @@
 #include <memory>
 #include <stdexcept>
 
-#include "rcpputils/scope_exit.hpp"
-
 using rclcpp::experimental::TimersManager;
 
 TimersManager::TimersManager(
   std::shared_ptr<rclcpp::Context> context,
-  std::function<void(const rclcpp::TimerBase *)> on_ready_callback)
+  std::function<void(void *)> on_ready_callback)
 {
   context_ = context;
   on_ready_callback_ = on_ready_callback;
@@ -125,6 +123,18 @@ size_t TimersManager::get_number_ready_timers()
   return locked_heap.get_number_ready_timers();
 }
 
+void TimersManager::execute_ready_timers()
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "execute_ready_timers() can't be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+  this->execute_ready_timers_unsafe();
+}
+
 bool TimersManager::execute_head_timer()
 {
   // Do not allow to interfere with the thread running
@@ -146,10 +156,12 @@ bool TimersManager::execute_head_timer()
 
   const bool timer_ready = head_timer->is_ready();
   if (timer_ready) {
-    // NOTE: here we always execute the timer, regardless of whether the
-    // on_ready_callback is set or not.
     head_timer->call();
-    head_timer->execute_callback();
+    if (on_ready_callback_) {
+      on_ready_callback_(head_timer.get());
+    } else {
+      head_timer->execute_callback();
+    }
     timers_heap.heapify_root();
     weak_timers_heap_.store(timers_heap);
   }
@@ -157,7 +169,7 @@ bool TimersManager::execute_head_timer()
   return timer_ready;
 }
 
-void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
+void TimersManager::execute_ready_timer(const void * timer_id)
 {
   TimerPtr ready_timer;
   {
@@ -234,10 +246,6 @@ void TimersManager::execute_ready_timers_unsafe()
 
 void TimersManager::run_timers()
 {
-  // Make sure the running flag is set to false when we exit from this function
-  // to allow restarting the timers thread.
-  RCPPUTILS_SCOPE_EXIT(this->running_.store(false); );
-
   while (rclcpp::ok(context_) && running_) {
     // Lock mutex
     std::unique_lock<std::mutex> lock(timers_mutex_);
@@ -261,6 +269,10 @@ void TimersManager::run_timers()
     // Execute timers
     this->execute_ready_timers_unsafe();
   }
+
+  // Make sure the running flag is set to false when we exit from this function
+  // to allow restarting the timers thread.
+  running_ = false;
 }
 
 void TimersManager::clear()
@@ -270,7 +282,7 @@ void TimersManager::clear()
     std::unique_lock<std::mutex> lock(timers_mutex_);
 
     TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
-    locked_heap.clear_timers_on_reset_callbacks();
+    locked_heap.clear_callbacks();
 
     weak_timers_heap_.clear();
 

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -25,20 +25,17 @@
 namespace rclcpp
 {
 
-std::shared_ptr<void>
-GenericSubscription::create_message()
+std::shared_ptr<void> GenericSubscription::create_message()
 {
   return create_serialized_message();
 }
 
-std::shared_ptr<rclcpp::SerializedMessage>
-GenericSubscription::create_serialized_message()
+std::shared_ptr<rclcpp::SerializedMessage> GenericSubscription::create_serialized_message()
 {
   return std::make_shared<rclcpp::SerializedMessage>(0);
 }
 
-void
-GenericSubscription::handle_message(
+void GenericSubscription::handle_message(
   std::shared_ptr<void> &,
   const rclcpp::MessageInfo &)
 {
@@ -54,8 +51,7 @@ GenericSubscription::handle_serialized_message(
   callback_(message);
 }
 
-void
-GenericSubscription::handle_loaned_message(
+void GenericSubscription::handle_loaned_message(
   void * message, const rclcpp::MessageInfo & message_info)
 {
   (void) message;
@@ -64,69 +60,16 @@ GenericSubscription::handle_loaned_message(
           "handle_loaned_message is not implemented for GenericSubscription");
 }
 
-void
-GenericSubscription::return_message(std::shared_ptr<void> & message)
+void GenericSubscription::return_message(std::shared_ptr<void> & message)
 {
   auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
   return_serialized_message(typed_message);
 }
 
-void
-GenericSubscription::return_serialized_message(
+void GenericSubscription::return_serialized_message(
   std::shared_ptr<rclcpp::SerializedMessage> & message)
 {
   message.reset();
 }
 
-
-// DYNAMIC TYPE ====================================================================================
-// TODO(methylDragon): Reorder later
-rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
-GenericSubscription::get_shared_dynamic_message_type()
-{
-  throw rclcpp::exceptions::UnimplementedError(
-          "get_shared_dynamic_message_type is not implemented for GenericSubscription");
-}
-
-rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-GenericSubscription::get_shared_dynamic_message()
-{
-  throw rclcpp::exceptions::UnimplementedError(
-          "get_shared_dynamic_message is not implemented for GenericSubscription");
-}
-
-rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
-GenericSubscription::get_shared_dynamic_serialization_support()
-{
-  throw rclcpp::exceptions::UnimplementedError(
-          "get_shared_dynamic_serialization_support is not implemented for GenericSubscription");
-}
-
-rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
-GenericSubscription::create_dynamic_message()
-{
-  throw rclcpp::exceptions::UnimplementedError(
-          "create_dynamic_message is not implemented for GenericSubscription");
-}
-
-void
-GenericSubscription::return_dynamic_message(
-  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
-{
-  (void) message;
-  throw rclcpp::exceptions::UnimplementedError(
-          "return_dynamic_message is not implemented for GenericSubscription");
-}
-
-void
-GenericSubscription::handle_dynamic_message(
-  const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
-  const rclcpp::MessageInfo & message_info)
-{
-  (void) message;
-  (void) message_info;
-  throw rclcpp::exceptions::UnimplementedError(
-          "handle_dynamic_message is not implemented for GenericSubscription");
-}
-
 }  // namespace rclcpp

rclcpp/src/rclcpp/logger.cpp

@@ -125,18 +125,4 @@ Logger::set_level(Level level)
   }
 }
 
-Logger::Level
-Logger::get_effective_level() const
-{
-  int logger_level = rcutils_logging_get_logger_effective_level(get_name());
-
-  if (logger_level < 0) {
-    exceptions::throw_from_rcl_error(
-      RCL_RET_ERROR, "Couldn't get logger level",
-      rcutils_get_error_state(), rcutils_reset_error);
-  }
-
-  return static_cast<Level>(logger_level);
-}
-
 }  // namespace rclcpp

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -45,7 +45,7 @@ NodeBase::NodeBase(
   node_handle_(nullptr),
   default_callback_group_(default_callback_group),
   associated_with_executor_(false),
-  notify_guard_condition_(std::make_shared<rclcpp::GuardCondition>(context)),
+  notify_guard_condition_(context),
   notify_guard_condition_is_valid_(false)
 {
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
@@ -132,10 +132,8 @@ NodeBase::NodeBase(
   // Create the default callback group, if needed.
   if (nullptr == default_callback_group_) {
     using rclcpp::CallbackGroupType;
-    // Default callback group is mutually exclusive and automatically associated with
-    // any executors that this node is added to.
     default_callback_group_ =
-      NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true);
+      NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive);
   }
 
   // Indicate the notify_guard_condition is now valid.
@@ -204,27 +202,11 @@ NodeBase::create_callback_group(
   rclcpp::CallbackGroupType group_type,
   bool automatically_add_to_executor_with_node)
 {
-  auto weak_context = this->get_context()->weak_from_this();
-  auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr {
-      return weak_context.lock();
-    };
-
   auto group = std::make_shared<rclcpp::CallbackGroup>(
     group_type,
-    get_node_context,
     automatically_add_to_executor_with_node);
   std::lock_guard<std::mutex> lock(callback_groups_mutex_);
   callback_groups_.push_back(group);
-
-  // This guard condition is generally used to signal to this node's executor that a callback
-  // group has been added that should be considered for new entities.
-  // If this is creating the default callback group, then the notify guard condition won't be
-  // ready or needed yet, as the node is not done being constructed and therefore cannot be added.
-  // If the callback group is not automatically associated with this node's executors, then
-  // triggering the guard condition is also unnecessary, it will be manually added to an exector.
-  if (notify_guard_condition_is_valid_ && automatically_add_to_executor_with_node) {
-    this->trigger_notify_guard_condition();
-  }
   return group;
 }
 
@@ -271,29 +253,9 @@ NodeBase::get_notify_guard_condition()
   if (!notify_guard_condition_is_valid_) {
     throw std::runtime_error("failed to get notify guard condition because it is invalid");
   }
-  return *notify_guard_condition_;
-}
-
-rclcpp::GuardCondition::SharedPtr
-NodeBase::get_shared_notify_guard_condition()
-{
-  std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
-  if (!notify_guard_condition_is_valid_) {
-    return nullptr;
-  }
   return notify_guard_condition_;
 }
 
-void
-NodeBase::trigger_notify_guard_condition()
-{
-  std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
-  if (!notify_guard_condition_is_valid_) {
-    throw std::runtime_error("failed to trigger notify guard condition because it is invalid");
-  }
-  notify_guard_condition_->trigger();
-}
-
 bool
 NodeBase::get_use_intra_process_default() const
 {

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -533,8 +533,9 @@ NodeGraph::notify_graph_change()
     }
   }
   graph_cv_.notify_all();
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on graph change: ") + ex.what());
@@ -788,15 +789,3 @@ rclcpp::TopicEndpointInfo::qos_profile() const
 {
   return qos_profile_;
 }
-
-rosidl_type_hash_t &
-rclcpp::TopicEndpointInfo::topic_type_hash()
-{
-  return topic_type_hash_;
-}
-
-const rosidl_type_hash_t &
-rclcpp::TopicEndpointInfo::topic_type_hash() const
-{
-  return topic_type_hash_;
-}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -651,7 +651,7 @@ NodeParameters::undeclare_parameter(const std::string & name)
   }
   if (!parameter_info->second.descriptor.dynamic_typing) {
     throw rclcpp::exceptions::InvalidParameterTypeException{
-            name, "cannot undeclare a statically typed parameter"};
+            name, "cannot undeclare an statically typed parameter"};
   }
 
   parameters_.erase(parameter_info);
@@ -824,7 +824,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       auto it = parameters_.find(parameter.get_name());
       if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) {
         if (!it->second.descriptor.dynamic_typing) {
-          result.reason = "cannot undeclare a statically typed parameter";
+          result.reason = "cannot undeclare an statically typed parameter";
           result.successful = false;
           return result;
         }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -42,8 +42,9 @@ NodeServices::add_service(
   group->add_service(service_base_ptr);
 
   // Notify the executor that a new service was created using the parent Node.
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
@@ -68,8 +69,9 @@ NodeServices::add_client(
   group->add_client(client_base_ptr);
 
   // Notify the executor that a new client was created using the parent Node.
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -42,8 +42,9 @@ NodeTimers::add_timer(
   }
   callback_group->add_timer(timer);
 
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -70,8 +70,9 @@ NodeTopics::add_publisher(
   }
 
   // Notify the executor that a new publisher was created using the parent Node.
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
@@ -118,8 +119,9 @@ NodeTopics::add_subscription(
   }
 
   // Notify the executor that a new subscription was created using the parent Node.
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -42,8 +42,9 @@ NodeWaitables::add_waitable(
   group->add_waitable(waitable_ptr);
 
   // Notify the executor that a new waitable was created using the parent Node.
+  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_base_->trigger_notify_guard_condition();
+    node_gc.trigger();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/subscription_base.cpp

@@ -22,7 +22,6 @@
 
 #include "rcpputils/scope_exit.hpp"
 
-#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
@@ -33,8 +32,6 @@
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
-#include "rosidl_dynamic_typesupport/types.h"
-
 using rclcpp::SubscriptionBase;
 
 SubscriptionBase::SubscriptionBase(
@@ -44,7 +41,7 @@ SubscriptionBase::SubscriptionBase(
   const rcl_subscription_options_t & subscription_options,
   const SubscriptionEventCallbacks & event_callbacks,
   bool use_default_callbacks,
-  DeliveredMessageKind delivered_message_kind)
+  bool is_serialized)
 : node_base_(node_base),
   node_handle_(node_base_->get_shared_rcl_node_handle()),
   node_logger_(rclcpp::get_node_logger(node_handle_.get())),
@@ -52,7 +49,7 @@ SubscriptionBase::SubscriptionBase(
   intra_process_subscription_id_(0),
   event_callbacks_(event_callbacks),
   type_support_(type_support_handle),
-  delivered_message_type_(delivered_message_kind)
+  is_serialized_(is_serialized)
 {
   auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs)
     {
@@ -261,13 +258,7 @@ SubscriptionBase::get_message_type_support_handle() const
 bool
 SubscriptionBase::is_serialized() const
 {
-  return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE;
-}
-
-rclcpp::DeliveredMessageKind
-SubscriptionBase::get_subscription_type() const
-{
-  return delivered_message_type_;
+  return is_serialized_;
 }
 
 size_t
@@ -451,7 +442,8 @@ SubscriptionBase::set_content_filter(
   rcl_subscription_content_filter_options_t options =
     rcl_get_zero_initialized_subscription_content_filter_options();
 
-  std::vector<const char *> cstrings = get_c_vector_string(expression_parameters);
+  std::vector<const char *> cstrings =
+    get_c_vector_string(expression_parameters);
   rcl_ret_t ret = rcl_subscription_content_filter_options_init(
     subscription_handle_.get(),
     get_c_string(filter_expression),
@@ -523,14 +515,3 @@ SubscriptionBase::get_content_filter() const
 
   return ret_options;
 }
-
-
-// DYNAMIC TYPE ==================================================================================
-bool
-SubscriptionBase::take_dynamic_message(
-  rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/,
-  rclcpp::MessageInfo & /*message_info_out*/)
-{
-  throw std::runtime_error("Unimplemented");
-  return false;
-}

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -362,3 +362,42 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark
     }
   }
 }
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_executor_entities_collector_execute)(benchmark::State & st)
+{
+  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
+    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator);
+  if (ret != RCL_RET_OK) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+  RCPPUTILS_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
+    if (ret != RCL_RET_OK) {
+      st.SkipWithError(rcutils_get_error_string().str);
+    }
+  });
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    (void)_;
+    std::shared_ptr<void> data = entities_collector_->take_data();
+    entities_collector_->execute(data);
+  }
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -671,6 +671,14 @@ if(TARGET test_executors)
   target_link_libraries(test_executors ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  ament_target_dependencies(test_static_single_threaded_executor
+    "test_msgs")
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick)
+endif()
+
 ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_multi_threaded_executor)
@@ -679,25 +687,17 @@ 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
+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_entities_collector)
-  ament_target_dependencies(test_entities_collector
+if(TARGET test_static_executor_entities_collector)
+  ament_target_dependencies(test_static_executor_entities_collector
     "rcl"
     "test_msgs")
-  target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick)
+  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
 endif()
 
-ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
-if(TARGET test_executor_notify_waitable)
-  ament_target_dependencies(test_executor_notify_waitable
-    "rcl"
-    "test_msgs")
-  target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5)
+ament_add_gtest(test_events_executor executors/test_events_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_events_executor)
   ament_target_dependencies(test_events_executor
     "test_msgs")

rclcpp/test/rclcpp/executors/test_entities_collector.cpp

@@ -1,320 +0,0 @@
-// Copyright 2023 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 "rclcpp/executors/executor_notify_waitable.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/executors/executor_entities_collector.hpp"
-
-#include "../../utils/rclcpp_gtest_macros.hpp"
-
-class TestExecutorEntitiesCollector : public ::testing::Test
-{
-public:
-  void SetUp()
-  {
-    rclcpp::init(0, nullptr);
-
-    notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-    entities_collector = std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(
-      notify_waitable);
-  }
-
-  void TearDown()
-  {
-    rclcpp::shutdown();
-  }
-
-  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable;
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector;
-};
-
-TEST_F(TestExecutorEntitiesCollector, add_remove_node) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-
-  // Add a node
-  EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector->update_collections());
-
-  // Remove a node
-  EXPECT_NO_THROW(entities_collector->remove_node(node1->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector->update_collections());
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_node_twice) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-
-  EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface()));
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector->add_node(node1->get_node_base_interface()),
-    std::runtime_error("Node '/ns/node1' has already been added to an executor."));
-
-  EXPECT_NO_THROW(entities_collector->update_collections());
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_associated_node) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-
-  // Simulate node being associated somewhere else
-  auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic();
-  has_executor.store(true);
-
-  // Add an already-associated node
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector->remove_node(node1->get_node_base_interface()),
-    std::runtime_error("Node '/ns/node1' needs to be associated with this executor."));
-
-  has_executor.store(false);
-}
-
-TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-
-  // Add an already-associated node
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector->remove_node(node1->get_node_base_interface()),
-    std::runtime_error("Node '/ns/node1' needs to be associated with an executor."));
-
-  // Simulate node being associated somewhere else
-  auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic();
-  has_executor.store(true);
-
-  // Add an already-associated node
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector->remove_node(node1->get_node_base_interface()),
-    std::runtime_error("Node '/ns/node1' needs to be associated with this executor."));
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_remove_node_before_update) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-
-  // Add and remove nodes without running updatenode
-  EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector.update_collections());
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_callback_group) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  // Add a callback group and update
-  entities_collector.add_callback_group(cb_group);
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  // Remove callback group and update
-  entities_collector.remove_callback_group(cb_group);
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  entities_collector.add_node(node->get_node_base_interface());
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 1u);
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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());
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 2u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 2u);
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector.add_callback_group(cb_group),
-    std::runtime_error("Callback group has already been added to an executor."));
-}
-
-TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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);
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  cb_group->get_associated_with_executor_atomic().exchange(false);
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector.add_callback_group(cb_group),
-    std::runtime_error("Callback group has already been added to this executor."));
-}
-
-TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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);
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  node.reset();
-
-  /**
-   * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed
-   * after their created callback groups.
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector.remove_callback_group(cb_group),
-    std::runtime_error("Node must not be deleted before its callback group(s)."));
-  */
-  EXPECT_NO_THROW(entities_collector.update_collections());
-}
-
-TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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);
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  EXPECT_NO_THROW(entities_collector.remove_callback_group(cb_group));
-
-  node.reset();
-
-  /**
-   * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed
-   * after their created callback groups.
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector.remove_callback_group(cb_group),
-    std::runtime_error("Node must not be deleted before its callback group(s)."));
-  */
-  EXPECT_NO_THROW(entities_collector.update_collections());
-}
-
-TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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);
-  entities_collector.update_collections();
-
-  ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u);
-  ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u);
-
-  entities_collector.remove_callback_group(cb_group);
-  entities_collector.update_collections();
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector.remove_callback_group(cb_group),
-    std::runtime_error("Callback group needs to be associated with an executor."));
-}
-
-TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) {
-  auto notify_waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-  auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable);
-
-  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_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface()));
-}

rclcpp/test/rclcpp/executors/test_events_executor.cpp

@@ -41,58 +41,6 @@ public:
   }
 };
 
-TEST_F(TestEventsExecutor, run_pub_sub)
-{
-  // rmw_connextdds doesn't support events-executor
-  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
-    GTEST_SKIP();
-  }
-
-  auto node = std::make_shared<rclcpp::Node>("node");
-
-  bool msg_received = false;
-  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
-    "topic", rclcpp::SensorDataQoS(),
-    [&msg_received](test_msgs::msg::Empty::ConstSharedPtr msg)
-    {
-      (void)msg;
-      msg_received = true;
-    });
-
-  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", rclcpp::SensorDataQoS());
-
-  EventsExecutor executor;
-  executor.add_node(node);
-
-  bool spin_exited = false;
-  std::thread spinner([&spin_exited, &executor, this]() {
-      executor.spin();
-      spin_exited = true;
-    });
-
-  auto msg = std::make_unique<test_msgs::msg::Empty>();
-  publisher->publish(std::move(msg));
-
-  // Wait some time for the subscription to receive the message
-  auto start = std::chrono::high_resolution_clock::now();
-  while (
-    !msg_received &&
-    !spin_exited &&
-    (std::chrono::high_resolution_clock::now() - start < 1s))
-  {
-    auto time = std::chrono::high_resolution_clock::now() - start;
-    auto time_msec = std::chrono::duration_cast<std::chrono::milliseconds>(time);
-    std::this_thread::sleep_for(25ms);
-  }
-
-  executor.cancel();
-  spinner.join();
-  executor.remove_node(node);
-
-  EXPECT_TRUE(msg_received);
-  EXPECT_TRUE(spin_exited);
-}
-
 TEST_F(TestEventsExecutor, run_clients_servers)
 {
   // rmw_connextdds doesn't support events-executor
@@ -188,6 +136,8 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timeout)
   EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
 }
 
+// FIX THIS TEST! The entities collector is being called too many times!
+/*
 TEST_F(TestEventsExecutor, spin_once_max_duration_timer)
 {
   // rmw_connextdds doesn't support events-executor
@@ -223,6 +173,7 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timer)
   EXPECT_EQ(1u, t_runs);
   EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
 }
+*/
 
 TEST_F(TestEventsExecutor, spin_some_max_duration)
 {
@@ -432,9 +383,6 @@ TEST_F(TestEventsExecutor, cancel_while_timers_waiting)
 
 TEST_F(TestEventsExecutor, destroy_entities)
 {
-  // This test fails on Windows! We skip it for now
-  GTEST_SKIP();
-
   // rmw_connextdds doesn't support events-executor
   if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
     GTEST_SKIP();
@@ -479,7 +427,9 @@ TEST_F(TestEventsExecutor, destroy_entities)
   spinner.join();
 }
 
-// Testing construction of a subscriptions with QoS event callback functions.
+/*
+   Testing construction of a subscriptions with QoS event callback functions.
+ */
 std::string * g_pub_log_msg;
 std::string * g_sub_log_msg;
 std::promise<void> * g_log_msgs_promise;

rclcpp/test/rclcpp/executors/test_events_queue.cpp

@@ -75,8 +75,8 @@ TEST(TestEventsQueue, SimpleQueueTest)
   simple_queue->enqueue(push_event);
   ret = simple_queue->dequeue(event);
   EXPECT_TRUE(ret);
-  EXPECT_EQ(push_event.entity_key, event.entity_key);
-  EXPECT_EQ(push_event.waitable_data, event.waitable_data);
+  EXPECT_EQ(push_event.exec_entity_id, event.exec_entity_id);
+  EXPECT_EQ(push_event.gen_entity_id, event.gen_entity_id);
   EXPECT_EQ(push_event.type, event.type);
   EXPECT_EQ(push_event.num_events, event.num_events);
 }

rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp

@@ -1,97 +0,0 @@
-// Copyright 2023 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/executors/single_threaded_executor.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-#include "rcpputils/scope_exit.hpp"
-
-#include "rclcpp/executors/executor_notify_waitable.hpp"
-
-#include "../../utils/rclcpp_gtest_macros.hpp"
-
-
-class TestExecutorNotifyWaitable : public ::testing::Test
-{
-public:
-  void SetUp()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  void TearDown()
-  {
-    rclcpp::shutdown();
-  }
-};
-
-TEST_F(TestExecutorNotifyWaitable, construct_destruct) {
-  {
-    auto waitable = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>();
-    waitable.reset();
-  }
-  {
-    auto on_execute_callback = []() {};
-    auto waitable =
-      std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(on_execute_callback);
-    waitable.reset();
-  }
-}
-
-TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) {
-  auto on_execute_callback = []() {};
-  auto waitable =
-    std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(on_execute_callback);
-
-  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-  auto notify_guard_condition =
-
-    node->get_node_base_interface()->get_shared_notify_guard_condition();
-  EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition));
-  EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition));
-}
-
-TEST_F(TestExecutorNotifyWaitable, wait) {
-  int on_execute_calls = 0;
-  auto on_execute_callback = [&on_execute_calls]() {on_execute_calls++;};
-
-  auto waitable =
-    std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(on_execute_callback);
-
-  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-  auto notify_guard_condition =
-    node->get_node_base_interface()->get_shared_notify_guard_condition();
-  EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition));
-
-  auto default_cbg = node->get_node_base_interface()->get_default_callback_group();
-  ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition());
-
-  auto waitables = node->get_node_waitables_interface();
-  waitables->add_waitable(std::static_pointer_cast<rclcpp::Waitable>(waitable), default_cbg);
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(node);
-  executor.spin_all(std::chrono::seconds(1));
-  EXPECT_EQ(1u, on_execute_calls);
-
-  // on_execute_callback doesn't change if the topology doesn't change
-  executor.spin_all(std::chrono::seconds(1));
-  EXPECT_EQ(1u, on_execute_calls);
-}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -83,6 +83,8 @@ public:
   int callback_count;
 };
 
+// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see:
+// https://github.com/ros2/rclcpp/issues/1219 for tracking
 template<typename T>
 class TestExecutorsStable : public TestExecutors<T> {};
 
@@ -124,6 +126,15 @@ public:
 // is updated.
 TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
 
+// StaticSingleThreadedExecutor is not included in these tests for now, due to:
+// https://github.com/ros2/rclcpp/issues/1219
+using StandardExecutors =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor,
+  rclcpp::experimental::executors::EventsExecutor>;
+TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames);
+
 // Make sure that executors detach from nodes when destructing
 TYPED_TEST(TestExecutors, detachOnDestruction)
 {
@@ -147,7 +158,10 @@ TYPED_TEST(TestExecutors, detachOnDestruction)
 }
 
 // Make sure that the executor can automatically remove expired nodes correctly
-TYPED_TEST(TestExecutors, addTemporaryNode) {
+// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see:
+// https://github.com/ros2/rclcpp/issues/1231
+TYPED_TEST(TestExecutorsStable, addTemporaryNode)
+{
   using ExecutorType = TypeParam;
   // rmw_connextdds doesn't support events-executor
   if (
@@ -173,25 +187,6 @@ TYPED_TEST(TestExecutors, addTemporaryNode) {
   spinner.join();
 }
 
-// Make sure that a spinning empty executor can be cancelled
-TYPED_TEST(TestExecutors, emptyExecutor)
-{
-  using ExecutorType = TypeParam;
-  // rmw_connextdds doesn't support events-executor
-  if (
-    std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>() &&
-    std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0)
-  {
-    GTEST_SKIP();
-  }
-
-  ExecutorType executor;
-  std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());});
-  std::this_thread::sleep_for(50ms);
-  executor.cancel();
-  spinner.join();
-}
-
 // Check executor throws properly if the same node is added a second time
 TYPED_TEST(TestExecutors, addNodeTwoExecutors)
 {
@@ -257,17 +252,14 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning)
   ExecutorType executor;
   executor.add_node(this->node);
 
-  std::atomic_bool timer_completed = false;
-  auto timer = this->node->create_wall_timer(
-    1ms, [&]() {
-      timer_completed.store(true);
-    });
+  bool timer_completed = false;
+  auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
 
   std::thread spinner([&]() {executor.spin();});
   // Sleep for a short time to verify executor.spin() is going, and didn't throw.
 
   auto start = std::chrono::steady_clock::now();
-  while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) {
+  while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) {
     std::this_thread::sleep_for(1ms);
   }
 
@@ -607,9 +599,8 @@ TYPED_TEST(TestExecutors, spinSome)
     this->publisher->publish(test_msgs::msg::Empty());
     std::this_thread::sleep_for(1ms);
   }
-  // The count of "execute" depends on whether the executor starts spinning before (1) or after (0)
-  // the first iteration of the while loop
-  EXPECT_LE(1u, my_waitable->get_count());
+
+  EXPECT_EQ(1u, my_waitable->get_count());
   waitable_interfaces->remove_waitable(my_waitable, nullptr);
   EXPECT_TRUE(spin_exited);
   // Cancel if it hasn't exited already.
@@ -758,106 +749,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr)
 
   rclcpp::shutdown();
 }
-
-template<typename T>
-class TestIntraprocessExecutors : public ::testing::Test
-{
-public:
-  static void SetUpTestCase()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
-  }
-
-  void SetUp()
-  {
-    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
-    std::stringstream test_name;
-    test_name << test_info->test_case_name() << "_" << test_info->name();
-    node = std::make_shared<rclcpp::Node>("node", test_name.str());
-
-    callback_count = 0;
-
-    const std::string topic_name = std::string("topic_") + test_name.str();
-
-    rclcpp::PublisherOptions po;
-    po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
-    publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, rclcpp::QoS(1), po);
-
-    auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {
-        this->callback_count.fetch_add(1);
-      };
-
-    rclcpp::SubscriptionOptions so;
-    so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
-    subscription =
-      node->create_subscription<test_msgs::msg::Empty>(
-      topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so);
-  }
-
-  void TearDown()
-  {
-    publisher.reset();
-    subscription.reset();
-    node.reset();
-  }
-
-  const size_t kNumMessages = 100;
-
-  rclcpp::Node::SharedPtr node;
-  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
-  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
-  std::atomic_int callback_count;
-};
-
-TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames);
-
-TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
-  // This tests that executors will continue to service intraprocess subscriptions in the case
-  // that publishers aren't continuing to publish.
-  // This was previously broken in that intraprocess guard conditions were only triggered on
-  // publish and the test was added to prevent future regressions.
-  const size_t kNumMessages = 100;
-
-  using ExecutorType = TypeParam;
-  ExecutorType executor;
-  executor.add_node(this->node);
-
-  EXPECT_EQ(0, this->callback_count.load());
-  this->publisher->publish(test_msgs::msg::Empty());
-
-  // Wait for up to 5 seconds for the first message to come available.
-  const std::chrono::milliseconds sleep_per_loop(10);
-  int loops = 0;
-  while (1u != this->callback_count.load() && loops < 500) {
-    rclcpp::sleep_for(sleep_per_loop);
-    executor.spin_some();
-    loops++;
-  }
-  EXPECT_EQ(1u, this->callback_count.load());
-
-  // reset counter
-  this->callback_count.store(0);
-
-  for (size_t ii = 0; ii < kNumMessages; ++ii) {
-    this->publisher->publish(test_msgs::msg::Empty());
-  }
-
-  // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read.
-  loops = 0;
-  auto timer = this->node->create_wall_timer(
-    std::chrono::milliseconds(10), [this, &executor, &loops, &kNumMessages]() {
-      loops++;
-      if (kNumMessages == this->callback_count.load() ||
-      loops == 500)
-      {
-        executor.cancel();
-      }
-    });
-  executor.spin();
-  EXPECT_EQ(kNumMessages, this->callback_count.load());
-}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -28,7 +28,6 @@
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rcutils/strdup.h"
-#include "test_msgs/msg/empty.h"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
@@ -600,18 +599,6 @@ TEST_F(TestNodeGraph, get_info_by_topic)
   rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile();
   EXPECT_EQ(const_actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable);
 
-  const rosidl_type_hash_t expected_type_hash = *test_msgs__msg__Empty__get_type_hash(nullptr);
-  EXPECT_EQ(
-    0, memcmp(
-      &publisher_endpoint_info.topic_type_hash(),
-      &expected_type_hash,
-      sizeof(rosidl_type_hash_t)));
-  EXPECT_EQ(
-    0, memcmp(
-      &const_publisher_endpoint_info.topic_type_hash(),
-      &expected_type_hash,
-      sizeof(rosidl_type_hash_t)));
-
   auto endpoint_gid = publisher_endpoint_info.endpoint_gid();
   auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid();
   bool endpoint_gid_is_all_zeros = true;

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -28,11 +28,6 @@
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
-using rclcpp::dynamic_typesupport::DynamicMessage;
-using rclcpp::dynamic_typesupport::DynamicMessageType;
-using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
-
-
 namespace
 {
 
@@ -82,18 +77,6 @@ public:
     const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &) override {}
   void return_message(std::shared_ptr<void> &) override {}
   void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> &) override {}
-
-  DynamicMessageType::SharedPtr get_shared_dynamic_message_type() override {return nullptr;}
-  DynamicMessage::SharedPtr get_shared_dynamic_message() override {return nullptr;}
-  DynamicSerializationSupport::SharedPtr get_shared_dynamic_serialization_support() override
-  {
-    return nullptr;
-  }
-  DynamicMessage::SharedPtr create_dynamic_message() override {return nullptr;}
-  void return_dynamic_message(DynamicMessage::SharedPtr &) override {}
-  void handle_dynamic_message(
-    const DynamicMessage::SharedPtr &,
-    const rclcpp::MessageInfo &) override {}
 };
 
 class TestNodeTopics : public ::testing::Test

rclcpp/test/rclcpp/test_executor.cpp

@@ -46,6 +46,23 @@ public:
   {
     spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));
   }
+
+  rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr()
+  {
+    return memory_strategy_.get();
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
+    rclcpp::CallbackGroup::SharedPtr group)
+  {
+    std::lock_guard<std::mutex> guard_{mutex_};  // only to make the TSA happy
+    return get_node_by_group(weak_groups_to_nodes_, group);
+  }
+
+  rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
+  {
+    return get_group_by_timer(timer);
+  }
 };
 
 class TestExecutor : public ::testing::Test
@@ -113,7 +130,7 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) {
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
     static_cast<void>(std::make_unique<DummyExecutor>()),
-    std::runtime_error("Failed to create wait set: error not set"));
+    std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
 }
 
 TEST_F(TestExecutor, add_callback_group_twice) {
@@ -125,7 +142,7 @@ TEST_F(TestExecutor, add_callback_group_twice) {
   cb_group->get_associated_with_executor_atomic().exchange(false);
   RCLCPP_EXPECT_THROW_EQ(
     dummy.add_callback_group(cb_group, node->get_node_base_interface(), false),
-    std::runtime_error("Callback group has already been added to this executor."));
+    std::runtime_error("Callback group was already added to executor."));
 }
 
 TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
@@ -151,15 +168,9 @@ TEST_F(TestExecutor, remove_callback_group_null_node) {
 
   node.reset();
 
-
-  /**
-   * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed
-   * after their created callback groups.
   RCLCPP_EXPECT_THROW_EQ(
     dummy.remove_callback_group(cb_group, false),
     std::runtime_error("Node must not be deleted before its callback group(s)."));
-   */
-  EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false));
 }
 
 TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
@@ -186,7 +197,7 @@ TEST_F(TestExecutor, remove_node_not_associated) {
 
   RCLCPP_EXPECT_THROW_EQ(
     dummy.remove_node(node->get_node_base_interface(), false),
-    std::runtime_error("Node '/ns/node' needs to be associated with an executor."));
+    std::runtime_error("Node needs to be associated with an executor."));
 }
 
 TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
@@ -200,7 +211,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
 
   RCLCPP_EXPECT_THROW_EQ(
     dummy2.remove_node(node1->get_node_base_interface(), false),
-    std::runtime_error("Node '/ns/node1' needs to be associated with this executor."));
+    std::runtime_error("Node needs to be associated with this executor."));
 }
 
 TEST_F(TestExecutor, spin_node_once_nanoseconds) {
@@ -317,14 +328,42 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) {
     std::runtime_error("Failed to trigger guard condition in cancel: error not set"));
 }
 
-TEST_F(TestExecutor, create_executor_fail_wait_set_clear) {
-  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+TEST_F(TestExecutor, set_memory_strategy_nullptr) {
+  DummyExecutor dummy;
+
   RCLCPP_EXPECT_THROW_EQ(
-    DummyExecutor dummy,
-    std::runtime_error("Couldn't clear the wait set: error not set"));
+    dummy.set_memory_strategy(nullptr),
+    std::runtime_error("Received NULL memory strategy in executor."));
 }
 
-TEST_F(TestExecutor, spin_all_fail_wait_set_clear) {
+TEST_F(TestExecutor, set_memory_strategy) {
+  DummyExecutor dummy;
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy =
+    std::make_shared<
+    rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+
+  dummy.set_memory_strategy(strategy);
+  EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get());
+}
+
+TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_once(std::chrono::milliseconds(1)),
+    std::runtime_error(
+      "Failed to trigger guard condition from execute_any_executable: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   auto timer =
@@ -332,10 +371,9 @@ TEST_F(TestExecutor, spin_all_fail_wait_set_clear) {
 
   dummy.add_node(node);
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
-
   RCLCPP_EXPECT_THROW_EQ(
-    dummy.spin_all(std::chrono::milliseconds(1)),
-    std::runtime_error("Couldn't clear the wait set: error not set"));
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't clear wait set: error not set"));
 }
 
 TEST_F(TestExecutor, spin_some_fail_wait_set_resize) {
@@ -363,7 +401,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) {
     RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
     dummy.spin_some(std::chrono::milliseconds(1)),
-    std::runtime_error("Couldn't fill wait set: error not set"));
+    std::runtime_error("Couldn't fill wait set"));
 }
 
 TEST_F(TestExecutor, spin_some_fail_wait) {
@@ -379,6 +417,71 @@ TEST_F(TestExecutor, spin_some_fail_wait) {
     std::runtime_error("rcl_wait() failed: error not set"));
 }
 
+TEST_F(TestExecutor, get_node_by_group_null_group) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr));
+}
+
+TEST_F(TestExecutor, get_node_by_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_node_by_group_not_found) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_nullptr) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
+}
+
+TEST_F(TestExecutor, get_group_by_timer) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  cb_group.reset();
+
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_add_callback_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
 TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
   DummyExecutor dummy;
   auto node = std::make_shared<rclcpp::Node>("node", "ns");

rclcpp/test/rclcpp/test_logger.cpp

@@ -160,50 +160,6 @@ TEST(TestLogger, set_level) {
   EXPECT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown());
 }
 
-TEST(TestLogger, get_effective_level) {
-  ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize());
-
-  rclcpp::Logger logger = rclcpp::get_logger("test_logger");
-  rclcpp::Logger child_logger = rclcpp::get_logger("test_logger.child");
-
-  // set child logger level unset to test effective level
-  child_logger.set_level(rclcpp::Logger::Level::Unset);
-
-  // default
-  EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level());
-
-  // unset
-  logger.set_level(rclcpp::Logger::Level::Unset);
-  EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level());
-
-  // debug
-  logger.set_level(rclcpp::Logger::Level::Debug);
-  EXPECT_EQ(rclcpp::Logger::Level::Debug, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Debug, child_logger.get_effective_level());
-
-  // info
-  logger.set_level(rclcpp::Logger::Level::Info);
-  EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level());
-
-  // warn
-  logger.set_level(rclcpp::Logger::Level::Warn);
-  EXPECT_EQ(rclcpp::Logger::Level::Warn, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Warn, child_logger.get_effective_level());
-
-  // error
-  logger.set_level(rclcpp::Logger::Level::Error);
-  EXPECT_EQ(rclcpp::Logger::Level::Error, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Error, child_logger.get_effective_level());
-
-  // fatal
-  logger.set_level(rclcpp::Logger::Level::Fatal);
-  EXPECT_EQ(rclcpp::Logger::Level::Fatal, logger.get_effective_level());
-  EXPECT_EQ(rclcpp::Logger::Level::Fatal, child_logger.get_effective_level());
-}
-
 TEST(TestLogger, get_logging_directory) {
   ASSERT_EQ(true, rcutils_set_env("HOME", "/fake_home_dir"));
   ASSERT_EQ(true, rcutils_set_env("USERPROFILE", nullptr));

rclcpp/test/rclcpp/test_timers_manager.cpp

@@ -42,14 +42,6 @@ public:
   }
 };
 
-static void execute_all_ready_timers(std::shared_ptr<TimersManager> timers_manager)
-{
-  bool head_was_ready = false;
-  do {
-    head_was_ready = timers_manager->execute_head_timer();
-  } while (head_was_ready);
-}
-
 TEST_F(TestTimersManager, empty_manager)
 {
   auto timers_manager = std::make_shared<TimersManager>(
@@ -57,6 +49,7 @@ TEST_F(TestTimersManager, empty_manager)
 
   EXPECT_EQ(std::chrono::nanoseconds::max(), timers_manager->get_head_timeout());
   EXPECT_FALSE(timers_manager->execute_head_timer());
+  EXPECT_NO_THROW(timers_manager->execute_ready_timers());
   EXPECT_NO_THROW(timers_manager->clear());
   EXPECT_NO_THROW(timers_manager->start());
   EXPECT_NO_THROW(timers_manager->stop());
@@ -66,7 +59,7 @@ TEST_F(TestTimersManager, add_run_remove_timer)
 {
   size_t t_runs = 0;
   auto t = TimerT::make_shared(
-    10ms,
+    1ms,
     [&t_runs]() {
       t_runs++;
     },
@@ -79,10 +72,10 @@ TEST_F(TestTimersManager, add_run_remove_timer)
   timers_manager->add_timer(t);
 
   // Sleep for more 3 times the timer period
-  std::this_thread::sleep_for(30ms);
+  std::this_thread::sleep_for(3ms);
 
   // The timer is executed only once, even if we slept 3 times the period
-  execute_all_ready_timers(timers_manager);
+  timers_manager->execute_ready_timers();
   EXPECT_EQ(1u, t_runs);
 
   // Remove the timer from the manager
@@ -143,11 +136,13 @@ TEST_F(TestTimersManager, timers_thread_exclusive_usage)
 
   EXPECT_THROW(timers_manager->start(), std::exception);
   EXPECT_THROW(timers_manager->get_head_timeout(), std::exception);
+  EXPECT_THROW(timers_manager->execute_ready_timers(), std::exception);
   EXPECT_THROW(timers_manager->execute_head_timer(), std::exception);
 
   timers_manager->stop();
 
   EXPECT_NO_THROW(timers_manager->get_head_timeout());
+  EXPECT_NO_THROW(timers_manager->execute_ready_timers());
   EXPECT_NO_THROW(timers_manager->execute_head_timer());
 }
 
@@ -226,19 +221,19 @@ TEST_F(TestTimersManager, timers_order)
   timers_manager->add_timer(t1);
 
   std::this_thread::sleep_for(10ms);
-  execute_all_ready_timers(timers_manager);
+  timers_manager->execute_ready_timers();
   EXPECT_EQ(1u, t1_runs);
   EXPECT_EQ(0u, t2_runs);
   EXPECT_EQ(0u, t3_runs);
 
   std::this_thread::sleep_for(30ms);
-  execute_all_ready_timers(timers_manager);
+  timers_manager->execute_ready_timers();
   EXPECT_EQ(2u, t1_runs);
   EXPECT_EQ(1u, t2_runs);
   EXPECT_EQ(0u, t3_runs);
 
   std::this_thread::sleep_for(100ms);
-  execute_all_ready_timers(timers_manager);
+  timers_manager->execute_ready_timers();
   EXPECT_EQ(3u, t1_runs);
   EXPECT_EQ(2u, t2_runs);
   EXPECT_EQ(1u, t3_runs);
@@ -246,7 +241,7 @@ TEST_F(TestTimersManager, timers_order)
   timers_manager->remove_timer(t1);
 
   std::this_thread::sleep_for(30ms);
-  execute_all_ready_timers(timers_manager);
+  timers_manager->execute_ready_timers();
   EXPECT_EQ(3u, t1_runs);
   EXPECT_EQ(3u, t2_runs);
   EXPECT_EQ(1u, t3_runs);
@@ -409,11 +404,23 @@ TEST_F(TestTimersManager, infinite_loop)
   timers_manager->add_timer(t1);
   timers_manager->add_timer(t2);
 
-  // Start a timers thread and make sure that we can stop it later
+  // Sleep for enough time to trigger timers
+  std::this_thread::sleep_for(3ms);
+  timers_manager->execute_ready_timers();
+  EXPECT_EQ(1u, t1_runs);
+  EXPECT_EQ(1u, t2_runs);
+
+  // Due to the long execution of timer callbacks, timers are already ready
+  bool ret = timers_manager->execute_head_timer();
+  EXPECT_TRUE(ret);
+  EXPECT_EQ(3u, t1_runs + t2_runs);
+
+  // Start a timers thread
   timers_manager->start();
-  std::this_thread::sleep_for(50ms);
+  std::this_thread::sleep_for(10ms);
   timers_manager->stop();
 
-  EXPECT_LT(0u, t1_runs);
-  EXPECT_LT(0u, t2_runs);
+  EXPECT_LT(3u, t1_runs + t2_runs);
+  EXPECT_LT(1u, t1_runs);
+  EXPECT_LT(1u, t2_runs);
 }

rclcpp_action/CHANGELOG.rst

@@ -3,13 +3,6 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
-20.0.0 (2023-04-13)
--------------------
-* extract the result response before the callback is issued. (`#2132 <https://github.com/ros2/rclcpp/issues/2132>`_)
-* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
-* Fix the GoalUUID to_string representation (`#1999 <https://github.com/ros2/rclcpp/issues/1999>`_)
-* Contributors: Chris Lalancette, Nathan Wiebe Neufeldt, Tomoya Fujita
-
 19.3.0 (2023-03-01)
 -------------------
 

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>20.0.0</version>
+  <version>19.3.0</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_components/CHANGELOG.rst

@@ -2,11 +2,6 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-20.0.0 (2023-04-13)
--------------------
-* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
-* Contributors: Chris Lalancette
-
 19.3.0 (2023-03-01)
 -------------------
 

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>20.0.0</version>
+  <version>19.3.0</version>
   <description>Package containing tools for dynamically loadable components</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,14 +3,6 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
-20.0.0 (2023-04-13)
--------------------
-* Fixes to silence some clang warnings. (`#2127 <https://github.com/ros2/rclcpp/issues/2127>`_)
-* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
-* Use the correct macro for LifecycleNode::get_fully_qualified_name (`#2117 <https://github.com/ros2/rclcpp/issues/2117>`_)
-* add get_fully_qualified_name to rclcpp_lifecycle (`#2115 <https://github.com/ros2/rclcpp/issues/2115>`_)
-* Contributors: Chris Lalancette, Steve Macenski
-
 19.3.0 (2023-03-01)
 -------------------
 

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp

@@ -91,24 +91,6 @@ public:
     rclcpp::Publisher<MessageT, Alloc>::publish(msg);
   }
 
-  /// LifecyclePublisher publish function
-  /**
-   * The publish function checks whether the communication
-   * was enabled or disabled and forwards the message
-   * to the actual rclcpp Publisher base class
-   */
-  virtual void
-  publish(
-    rclcpp::LoanedMessage<typename rclcpp::Publisher<MessageT,
-    Alloc>::ROSMessageType, Alloc> && loaned_msg)
-  {
-    if (!this->is_activated()) {
-      log_publisher_not_enabled();
-      return;
-    }
-    rclcpp::Publisher<MessageT, Alloc>::publish(std::move(loaned_msg));
-  }
-
   void
   on_activate() override
   {

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>20.0.0</version>
+  <version>19.3.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -129,10 +129,6 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
-  {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
-  }
   node_->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret);
   ASSERT_EQ(success, ret);
@@ -147,10 +143,6 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
-  {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
-  }
 }
 
 TEST_P(TestLifecyclePublisher, publish) {
@@ -165,10 +157,6 @@ TEST_P(TestLifecyclePublisher, publish) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
-  {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
-  }
   node_->publisher()->on_activate();
   EXPECT_TRUE(node_->publisher()->is_activated());
   {
@@ -179,10 +167,6 @@ TEST_P(TestLifecyclePublisher, publish) {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
     EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
   }
-  {
-    auto loaned_msg = node_->publisher()->borrow_loaned_message();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg)));
-  }
 }
 
 INSTANTIATE_TEST_SUITE_P(