21.0.7

Signed-off-by: Yadunund <yadunund@gmail.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,79 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+21.0.7 (2024-07-10)
+-------------------
+* Add test creating two content filter topics with the same topic name (`#2550 <https://github.com/ros2/rclcpp/issues/2550>`_)
+* Revise the description of service configure_introspection() (`#2514 <https://github.com/ros2/rclcpp/issues/2514>`_)
+* Contributors: Alejandro Hernández Cordero, Barry Xu
+
+21.0.6 (2024-04-19)
+-------------------
+* address ambiguous auto variable. (`#2486 <https://github.com/ros2/rclcpp/issues/2486>`_)
+* Contributors: Tomoya Fujita
+
+21.0.5 (2024-02-07)
+-------------------
+* Fix data race in EventHandlerBase (`#2387 <https://github.com/ros2/rclcpp/issues/2387>`_)
+* Contributors: mauropasse
+
+21.0.4 (2023-11-17)
+-------------------
+* Disable the loaned messages inside the executor. (`#2365 <https://github.com/ros2/rclcpp/issues/2365>`_)
+* Add missing 'enable_rosout' comments (`#2346 <https://github.com/ros2/rclcpp/issues/2346>`_)
+* Address rate related flaky tests. (`#2341 <https://github.com/ros2/rclcpp/issues/2341>`_)
+* Add missing stdexcept include (`#2333 <https://github.com/ros2/rclcpp/issues/2333>`_)
+* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2322 <https://github.com/ros2/rclcpp/issues/2322>`_)
+* Fix C++20 allocator construct deprecation (`#2318 <https://github.com/ros2/rclcpp/issues/2318>`_)
+* Topic correct typeadapter deduction (`#2298 <https://github.com/ros2/rclcpp/issues/2298>`_)
+* Contributors: AiVerisimilitude, Chen Lihui, Chris Lalancette, Jiaqi Li, Øystein Sture, Tomoya Fujita, William Woodall
+
+21.0.3 (2023-09-08)
+-------------------
+* Do not crash Executor when send_response fails due to client failure. (`#2279 <https://github.com/ros2/rclcpp/issues/2279>`_)
+* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2236 <https://github.com/ros2/rclcpp/issues/2236>`_)
+* Contributors: Emerson Knapp, Tomoya Fujita, Zang MingJie
+
+21.0.2 (2023-07-14)
+-------------------
+* Fix warnings related to comparison of integer expressions of different signedness (`#2222 <https://github.com/ros2/rclcpp/issues/2222>`_)
+* Fix race condition in events-executor (`#2191 <https://github.com/ros2/rclcpp/issues/2191>`_)
+* Contributors: Alberto Soragna, Tomoya Fujita
+
+21.0.1 (2023-05-11)
+-------------------
+* Fix delivered message kind (`#2175 <https://github.com/ros2/rclcpp/issues/2175>`_) (`#2178 <https://github.com/ros2/rclcpp/issues/2178>`_)
+* Contributors: mergify[bot]
+
+21.0.0 (2023-04-18)
+-------------------
+* Add support for logging service. (`#2122 <https://github.com/ros2/rclcpp/issues/2122>`_)
+* Picking ABI-incompatible executor changes (`#2170 <https://github.com/ros2/rclcpp/issues/2170>`_)
+* add events-executor and timers-manager in rclcpp (`#2155 <https://github.com/ros2/rclcpp/issues/2155>`_)
+* Create common structures for executors to use (`#2143 <https://github.com/ros2/rclcpp/issues/2143>`_)
+* Implement deliver message kind (`#2168 <https://github.com/ros2/rclcpp/issues/2168>`_)
+* Contributors: Alberto Soragna, Lei Liu, Michael Carroll, methylDragon
+
+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,16 +49,26 @@ 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/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
@@ -82,6 +92,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_time_source.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_type_descriptions.cpp
   src/rclcpp/node_interfaces/node_waitables.cpp
   src/rclcpp/node_options.cpp
   src/rclcpp/parameter.cpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -93,11 +93,54 @@ 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();
@@ -137,6 +180,13 @@ 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;
+
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
@@ -178,11 +228,24 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
-  /// Defer creating the notify guard condition and return it.
+  /// 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")]]
   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
@@ -234,6 +297,8 @@ 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/client.hpp

@@ -845,7 +845,7 @@ protected:
         "Received invalid sequence number. Ignoring...");
       return std::nullopt;
     }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
     this->pending_requests_.erase(request_number);
     return value;
   }

rclcpp/include/rclcpp/context.hpp

@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -0,0 +1,70 @@
+// 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

@@ -0,0 +1,64 @@
+// 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

@@ -0,0 +1,65 @@
+// 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

@@ -0,0 +1,67 @@
+// 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

@@ -0,0 +1,60 @@
+// 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/event_handler.hpp

@@ -260,6 +260,16 @@ public:
     }
   }
 
+  ~EventHandler()
+  {
+    // Since the rmw event listener holds a reference to the
+    // "on ready" callback, we need to clear it on destruction of this class.
+    // This clearing is not needed for other rclcpp entities like pub/subs, since
+    // they do own the underlying rmw entities, which are destroyed
+    // on their rclcpp destructors, thus no risk of dangling pointers.
+    clear_on_ready_callback();
+  }
+
   /// Take data so that the callback cannot be scheduled again
   std::shared_ptr<void>
   take_data() override

rclcpp/include/rclcpp/executor.hpp

@@ -51,6 +51,7 @@ typedef std::map<rclcpp::CallbackGroup::WeakPtr,
 
 // Forward declaration is used in convenience method signature.
 class Node;
+class ExecutorImplementation;
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -637,8 +638,9 @@ protected:
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rclcpp::GuardCondition interrupt_guard_condition_;
+  std::shared_ptr<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.
@@ -696,6 +698,9 @@ protected:
 
   /// 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.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"

rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

@@ -0,0 +1,213 @@
+// 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

@@ -0,0 +1,270 @@
+// 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

@@ -0,0 +1,158 @@
+// 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/experimental/buffers/intra_process_buffer.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
 #include "rclcpp/macros.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -94,6 +95,10 @@ 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,6 +25,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -51,6 +52,7 @@ 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() {}
@@ -67,6 +69,12 @@ 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_);
@@ -90,6 +98,11 @@ 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_--;
@@ -135,7 +148,10 @@ public:
     return is_full_();
   }
 
-  void clear() {}
+  void clear()
+  {
+    TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+  }
 
 private:
   /// Get the next index value for the ring buffer

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

@@ -0,0 +1,291 @@
+// 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_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+#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_event_types.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"
+#include "rclcpp/node.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/// Events executor implementation
+/**
+ * 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.
+ *
+ * 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
+ * as very low CPU usage, bounded memory requirement, determinism, etc.
+ *
+ * The executor uses a weak ownership model and it locks entities only while executing
+ * their related events.
+ *
+ * To run this executor:
+ * rclcpp::experimental::executors::EventsExecutor executor;
+ * executor.add_node(node);
+ * executor.spin();
+ * executor.remove_node(node);
+ */
+class EventsExecutor : public rclcpp::Executor
+{
+  friend class EventsExecutorEntitiesCollector;
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
+
+  /// Default constructor. See the default constructor for Executor.
+  /**
+   * \param[in] events_queue The queue used to store events.
+   * \param[in] execute_timers_separate_thread If true, timers are executed in a separate
+   * thread. If false, timers are executed in the same thread as all other entities.
+   * \param[in] options Options used to configure the executor.
+   */
+  RCLCPP_PUBLIC
+  explicit EventsExecutor(
+    rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
+      rclcpp::experimental::executors::SimpleEventsQueue>(),
+    bool execute_timers_separate_thread = false,
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
+
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  virtual ~EventsExecutor();
+
+  /// Events executor implementation of spin.
+  /**
+   * This function will block until work comes in, execute it, and keep blocking.
+   * It will only be interrupted by a CTRL-C (managed by the global signal handler).
+   * \throws std::runtime_error when spin() called while already spinning
+   */
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+
+  /// Events executor implementation of spin some
+  /**
+   * This non-blocking function will execute the timers and events
+   * that were ready when this API was called, until timeout or no
+   * more work available. New ready-timers/events arrived while
+   * executing work, won't be taken into account here.
+   *
+   * Example:
+   *   while(condition) {
+   *     spin_some();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
+
+  /// Events executor implementation of spin all
+  /**
+   * This non-blocking function will execute timers and events
+   * until timeout or no more work available. If new ready-timers/events
+   * arrive while executing work available, they will be executed
+   * as long as the timeout hasn't expired.
+   *
+   * Example:
+   *   while(condition) {
+   *     spin_all();
+   *     sleep(); // User should have some sync work or
+   *              // sleep to avoid a 100% CPU usage
+   *   }
+   */
+  RCLCPP_PUBLIC
+  void
+  spin_all(std::chrono::nanoseconds max_duration) 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::EventsExecutor::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;
+
+  /// 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;
+
+  /// 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;
+
+  /// 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:
+  /// 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);
+
+private:
+  RCLCPP_DISABLE_COPY(EventsExecutor)
+
+  /// Execute a provided executor event if its associated entities are available
+  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);
+
+  /// Utility to add the notify waitable to an entities collection
+  void
+  add_notify_waitable_to_collection(
+    rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection);
+
+  /// 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
+  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_;
+
+  /// 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
+  std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_

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

@@ -0,0 +1,46 @@
+// 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_EVENT_TYPES_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+enum ExecutorEventType
+{
+  CLIENT_EVENT,
+  SUBSCRIPTION_EVENT,
+  SERVICE_EVENT,
+  TIMER_EVENT,
+  WAITABLE_EVENT
+};
+
+struct ExecutorEvent
+{
+  const void * entity_key;
+  int waitable_data;
+  ExecutorEventType type;
+  size_t num_events;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_

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

@@ -0,0 +1,100 @@
+// 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_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
+
+#include <queue>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This abstract class can be used to implement different types of queues
+ * where `ExecutorEvent` can be stored.
+ * The derived classes should choose which underlying container to use and
+ * the strategy for pushing and popping events.
+ * For example a queue implementation may be bounded or unbounded and have
+ * different pruning strategies.
+ * Implementations may or may not check the validity of events and decide how to handle
+ * the situation where an event is not valid anymore (e.g. a subscription history cache overruns)
+ */
+class EventsQueue
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue)
+
+  RCLCPP_PUBLIC
+  EventsQueue() = default;
+
+  /**
+   * @brief Destruct the object.
+   */
+  RCLCPP_PUBLIC
+  virtual ~EventsQueue() = default;
+
+  /**
+   * @brief push event into the queue
+   * @param event The event to push into the queue
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0;
+
+  /**
+   * @brief Extracts an event from the queue, eventually waiting until timeout
+   * if none is available.
+   * @return true if event has been found, false if timeout
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  dequeue(
+    rclcpp::experimental::executors::ExecutorEvent & event,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0;
+
+  /**
+   * @brief Test whether queue is empty
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  empty() const = 0;
+
+  /**
+   * @brief Returns the number of elements in the queue.
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  size() const = 0;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_

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

@@ -0,0 +1,134 @@
+// 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__SIMPLE_EVENTS_QUEUE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
+
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+#include <utility>
+
+#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class implements an EventsQueue as a simple wrapper around a std::queue.
+ * It does not perform any checks about the size of queue, which can grow
+ * unbounded without being pruned.
+ * The simplicity of this implementation makes it suitable for optimizing CPU usage.
+ */
+class SimpleEventsQueue : public EventsQueue
+{
+public:
+  RCLCPP_PUBLIC
+  ~SimpleEventsQueue() override = default;
+
+  /**
+   * @brief enqueue event into the queue
+   * Thread safe
+   * @param event The event to enqueue into the queue
+   */
+  RCLCPP_PUBLIC
+  void
+  enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override
+  {
+    rclcpp::experimental::executors::ExecutorEvent single_event = event;
+    single_event.num_events = 1;
+    {
+      std::unique_lock<std::mutex> lock(mutex_);
+      for (size_t ev = 0; ev < event.num_events; ev++) {
+        event_queue_.push(single_event);
+      }
+    }
+    events_queue_cv_.notify_one();
+  }
+
+  /**
+   * @brief waits for an event until timeout, gets a single event
+   * Thread safe
+   * @return true if event, false if timeout
+   */
+  RCLCPP_PUBLIC
+  bool
+  dequeue(
+    rclcpp::experimental::executors::ExecutorEvent & event,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    // Initialize to true because it's only needed if we have a valid timeout
+    bool has_data = true;
+    if (timeout != std::chrono::nanoseconds::max()) {
+      has_data =
+        events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();});
+    } else {
+      events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();});
+    }
+
+    if (has_data) {
+      event = event_queue_.front();
+      event_queue_.pop();
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * @brief Test whether queue is empty
+   * Thread safe
+   * @return true if the queue's size is 0, false otherwise.
+   */
+  RCLCPP_PUBLIC
+  bool
+  empty() const override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return event_queue_.empty();
+  }
+
+  /**
+   * @brief Returns the number of elements in the queue.
+   * Thread safe
+   * @return the number of elements in the queue.
+   */
+  RCLCPP_PUBLIC
+  size_t
+  size() const override
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return event_queue_.size();
+  }
+
+private:
+  // The underlying queue implementation
+  std::queue<rclcpp::experimental::executors::ExecutorEvent> event_queue_;
+  // Mutex to protect read/write access to the queue
+  mutable std::mutex mutex_;
+  // Variable used to notify when an event is added to the queue
+  std::condition_variable events_queue_cv_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -481,13 +481,13 @@ private:
                 "subscription use different allocator types, which is not supported");
       }
 
-      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+      if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
         ROSMessageTypeAllocator ros_message_alloc(allocator);
-        auto ptr = ros_message_alloc.allocate(1);
-        ros_message_alloc.construct(ptr);
+        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
+        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
         ROSMessageTypeDeleter deleter;
         allocator::set_allocator_for_deleter(&deleter, &allocator);
-        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *ptr);
+        rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(*message, *ptr);
         auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
         ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
       } else {

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -118,6 +118,13 @@ 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,6 +31,8 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rclcpp
 {
 namespace experimental
@@ -91,6 +93,10 @@ 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

@@ -0,0 +1,553 @@
+// 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__TIMERS_MANAGER_HPP_
+#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
+
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/timer.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+/**
+ * @brief This class provides a way for storing and executing timer objects.
+ * It provides APIs to suit the needs of different applications and execution models.
+ * All public APIs provided by this class are thread-safe.
+ *
+ * Timers management
+ * This class provides APIs to add/remove timers to/from an internal storage.
+ * It keeps a list of weak pointers from added timers, and locks them only when
+ * they need to be executed or modified.
+ * Timers are kept ordered in a binary-heap priority queue.
+ * Calls to add/remove APIs will temporarily block the execution of the timers and
+ * will require to reorder the internal priority queue.
+ * 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.
+ *
+ * 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,
+ * so timers may be executed out of order, without this object noticing it.
+ *
+ */
+class TimersManager
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager)
+
+  /**
+   * @brief Construct a new TimersManager object
+   *
+   * @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.
+   */
+  RCLCPP_PUBLIC
+  TimersManager(
+    std::shared_ptr<rclcpp::Context> context,
+    std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
+
+  /**
+   * @brief Destruct the TimersManager object making sure to stop thread and release memory.
+   */
+  RCLCPP_PUBLIC
+  ~TimersManager();
+
+  /**
+   * @brief Adds a new timer to the storage, maintaining weak ownership of it.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   *
+   * @param timer the timer to add.
+   * @throws std::invalid_argument if timer is a nullptr.
+   */
+  RCLCPP_PUBLIC
+  void add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  /**
+   * @brief Remove a single timer from the object storage.
+   * Will do nothing if the timer was not being stored here.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   *
+   * @param timer the timer to remove.
+   */
+  RCLCPP_PUBLIC
+  void remove_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  /**
+   * @brief Remove all the timers stored in the object.
+   * Function is thread safe and it can be called regardless of the state of the timers thread.
+   */
+  RCLCPP_PUBLIC
+  void clear();
+
+  /**
+   * @brief Starts a thread that takes care of executing the timers stored in this object.
+   * Function will throw an error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  void start();
+
+  /**
+   * @brief Stops the timers thread.
+   * Will do nothing if the timer thread was not running.
+   */
+  RCLCPP_PUBLIC
+  void stop();
+
+  /**
+   * @brief Get the number of timers that are currently ready.
+   * This function is thread safe.
+   *
+   * @return size_t number of ready timers.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  size_t get_number_ready_timers();
+
+  /**
+   * @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.
+   */
+  RCLCPP_PUBLIC
+  bool execute_head_timer();
+
+  /**
+   * @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);
+
+  /**
+   * @brief Get the amount of time before the next timer triggers.
+   * This function is thread safe.
+   *
+   * @return std::chrono::nanoseconds to wait,
+   * the returned value could be negative if the timer is already expired
+   * or std::chrono::nanoseconds::max() if there are no timers stored in the object.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  std::chrono::nanoseconds get_head_timeout();
+
+private:
+  RCLCPP_DISABLE_COPY(TimersManager)
+
+  using TimerPtr = rclcpp::TimerBase::SharedPtr;
+  using WeakTimerPtr = rclcpp::TimerBase::WeakPtr;
+
+  // Forward declaration
+  class TimersHeap;
+
+  /**
+   * @brief This class allows to store weak pointers to timers in a heap-like data structure.
+   * The root of the heap is the timer that triggers first.
+   * Since this class uses weak ownership, it is not guaranteed that it represents a valid heap
+   * at any point in time as timers could go out of scope, thus invalidating it.
+   * The "validate_and_lock" API allows to restore the heap property and also returns a locked version
+   * of the timers heap.
+   * This class is not thread safe and requires external mutexes to protect its usage.
+   */
+  class WeakTimersHeap
+  {
+public:
+    /**
+     * @brief Add a new timer to the heap. After the addition, the heap property is enforced.
+     *
+     * @param timer new timer to add.
+     * @return true if timer has been added, false if it was already there.
+     */
+    bool add_timer(TimerPtr timer)
+    {
+      TimersHeap locked_heap = this->validate_and_lock();
+      bool added = locked_heap.add_timer(std::move(timer));
+
+      if (added) {
+        // Re-create the weak heap with the new timer added
+        this->store(locked_heap);
+      }
+
+      return added;
+    }
+
+    /**
+     * @brief Remove a timer from the heap. After the removal, the heap property is enforced.
+     *
+     * @param timer timer to remove.
+     * @return true if timer has been removed, false if it was not there.
+     */
+    bool remove_timer(TimerPtr timer)
+    {
+      TimersHeap locked_heap = this->validate_and_lock();
+      bool removed = locked_heap.remove_timer(std::move(timer));
+
+      if (removed) {
+        // Re-create the weak heap with the timer removed
+        this->store(locked_heap);
+      }
+
+      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)
+    {
+      for (auto & weak_timer : weak_heap_) {
+        auto timer = weak_timer.lock();
+        if (timer.get() == timer_id) {
+          return timer;
+        }
+      }
+      return nullptr;
+    }
+
+    /**
+     * @brief Returns a const reference to the front element.
+     */
+    const WeakTimerPtr & front() const
+    {
+      return weak_heap_.front();
+    }
+
+    /**
+     * @brief Returns whether the heap is empty or not.
+     */
+    bool empty() const
+    {
+      return weak_heap_.empty();
+    }
+
+    /**
+     * @brief This function restores the current object as a valid heap
+     * and it returns a locked version of it.
+     * Timers that went out of scope are removed from the container.
+     * It is the only public API to access and manipulate the stored timers.
+     *
+     * @return TimersHeap owned timers corresponding to the current object
+     */
+    TimersHeap validate_and_lock()
+    {
+      TimersHeap locked_heap;
+      bool any_timer_destroyed = false;
+
+      for (auto weak_timer : weak_heap_) {
+        auto timer = weak_timer.lock();
+        if (timer) {
+          // This timer is valid, so add it to the locked heap
+          // Note: we access friend private `owned_heap_` member field.
+          locked_heap.owned_heap_.push_back(std::move(timer));
+        } else {
+          // This timer went out of scope, so we don't add it to locked heap
+          // and we mark the corresponding flag.
+          // It's not needed to erase it from weak heap, as we are going to re-heapify.
+          // Note: we can't exit from the loop here, as we need to find all valid timers.
+          any_timer_destroyed = true;
+        }
+      }
+
+      // If a timer has gone out of scope, then the remaining elements do not represent
+      // a valid heap anymore. We need to re-heapify the timers heap.
+      if (any_timer_destroyed) {
+        locked_heap.heapify();
+        // Re-create the weak heap now that elements have been heapified again
+        this->store(locked_heap);
+      }
+
+      return locked_heap;
+    }
+
+    /**
+     * @brief This function allows to recreate the heap of weak pointers
+     * from an heap of owned pointers.
+     * It is required to be called after a locked TimersHeap generated from this object
+     * has been modified in any way (e.g. timers triggered, added, removed).
+     *
+     * @param heap timers heap to store as weak pointers
+     */
+    void store(const TimersHeap & heap)
+    {
+      weak_heap_.clear();
+      // Note: we access friend private `owned_heap_` member field.
+      for (auto t : heap.owned_heap_) {
+        weak_heap_.push_back(t);
+      }
+    }
+
+    /**
+     * @brief Remove all timers from the heap.
+     */
+    void clear()
+    {
+      weak_heap_.clear();
+    }
+
+private:
+    std::vector<WeakTimerPtr> weak_heap_;
+  };
+
+  /**
+   * @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers.
+   * It can be generated by locking the weak version.
+   * It provides operations to manipulate the heap.
+   * This class is not thread safe and requires external mutexes to protect its usage.
+   */
+  class TimersHeap
+  {
+public:
+    /**
+     * @brief Try to add a new timer to the heap.
+     * After the addition, the heap property is preserved.
+     * @param timer new timer to add.
+     * @return true if timer has been added, false if it was already there.
+     */
+    bool add_timer(TimerPtr timer)
+    {
+      // Nothing to do if the timer is already stored here
+      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
+      if (it != owned_heap_.end()) {
+        return false;
+      }
+
+      owned_heap_.push_back(std::move(timer));
+      std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
+
+      return true;
+    }
+
+    /**
+     * @brief Try to remove a timer from the heap.
+     * After the removal, the heap property is preserved.
+     * @param timer timer to remove.
+     * @return true if timer has been removed, false if it was not there.
+     */
+    bool remove_timer(TimerPtr timer)
+    {
+      // Nothing to do if the timer is not stored here
+      auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
+      if (it == owned_heap_.end()) {
+        return false;
+      }
+
+      owned_heap_.erase(it);
+      this->heapify();
+
+      return true;
+    }
+
+    /**
+     * @brief Returns a reference to the front element.
+     * @return reference to front element.
+     */
+    TimerPtr & front()
+    {
+      return owned_heap_.front();
+    }
+
+    /**
+     * @brief Returns a const reference to the front element.
+     * @return const reference to front element.
+     */
+    const TimerPtr & front() const
+    {
+      return owned_heap_.front();
+    }
+
+    /**
+     * @brief Returns whether the heap is empty or not.
+     * @return true if the heap is empty.
+     */
+    bool empty() const
+    {
+      return owned_heap_.empty();
+    }
+
+    /**
+     * @brief Returns the size of the heap.
+     * @return the number of valid timers in the heap.
+     */
+    size_t size() const
+    {
+      return owned_heap_.size();
+    }
+
+    /**
+     * @brief Get the number of timers that are currently ready.
+     * @return size_t number of ready timers.
+     */
+    size_t get_number_ready_timers() const
+    {
+      size_t ready_timers = 0;
+
+      for (TimerPtr t : owned_heap_) {
+        if (t->is_ready()) {
+          ready_timers++;
+        }
+      }
+
+      return ready_timers;
+    }
+
+    /**
+    * @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered).
+    */
+    void heapify_root()
+    {
+      // The following code is a more efficient version than doing
+      // pop_heap, pop_back, push_back, push_heap
+      // as it removes the need for the last push_heap
+
+      // Push the modified element (i.e. the current root) at the bottom of the heap
+      owned_heap_.push_back(owned_heap_[0]);
+      // Exchange first and last-1 elements and reheapify
+      std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
+      // Remove last element
+      owned_heap_.pop_back();
+    }
+
+    /**
+     * @brief Completely restores the structure to a valid heap
+     */
+    void heapify()
+    {
+      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()
+    {
+      for (TimerPtr & t : owned_heap_) {
+        t->clear_on_reset_callback();
+      }
+    }
+
+    /**
+     * @brief Friend declaration to allow the `validate_and_lock()` function to access the
+     * underlying heap container
+     */
+    friend TimersHeap WeakTimersHeap::validate_and_lock();
+
+    /**
+     * @brief Friend declaration to allow the `store()` function to access the
+     * underlying heap container
+     */
+    friend void WeakTimersHeap::store(const TimersHeap & heap);
+
+private:
+    /**
+     * @brief Comparison function between timers.
+     * @return true if `a` triggers after `b`.
+     */
+    static bool timer_greater(TimerPtr a, TimerPtr b)
+    {
+      // TODO(alsora): this can cause an error if timers are using different clocks
+      return a->time_until_trigger() > b->time_until_trigger();
+    }
+
+    std::vector<TimerPtr> owned_heap_;
+  };
+
+  /**
+   * @brief Implements a loop that keeps executing ready timers.
+   * This function is executed in the timers thread.
+   */
+  void run_timers();
+
+  /**
+   * @brief Get the amount of time before the next timer triggers.
+   * This function is not thread safe, acquire a mutex before calling it.
+   *
+   * @return std::chrono::nanoseconds to wait,
+   * the returned value could be negative if the timer is already expired
+   * or std::chrono::nanoseconds::max() if the heap is empty.
+   * This function is not thread safe, acquire the timers_mutex_ before calling it.
+   */
+  std::chrono::nanoseconds get_head_timeout_unsafe();
+
+  /**
+   * @brief Executes all the timers currently ready when the function is invoked
+   * while keeping the heap correctly sorted.
+   * This function is not thread safe, acquire the timers_mutex_ before calling it.
+   */
+  void execute_ready_timers_unsafe();
+
+  // Callback to be called when timer is ready
+  std::function<void(const rclcpp::TimerBase *)> on_ready_callback_ = nullptr;
+
+  // Thread used to run the timers execution task
+  std::thread timers_thread_;
+  // Protects access to timers
+  std::mutex timers_mutex_;
+  // Protects access to stop()
+  std::mutex stop_mutex_;
+  // Notifies the timers thread whenever timers are added/removed
+  std::condition_variable timers_cv_;
+  // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
+  bool timers_updated_ {false};
+  // Indicates whether the timers thread is currently running or not
+  std::atomic<bool> running_ {false};
+  // Parent context used to understand if ROS is still active
+  std::shared_ptr<rclcpp::Context> context_;
+  // Timers heap storage with weak ownership
+  WeakTimersHeap weak_timers_heap_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -84,7 +84,7 @@ public:
       options.to_rcl_subscription_options(qos),
       options.event_callbacks,
       options.use_default_callbacks,
-      true),
+      DeliveredMessageKind::SERIALIZED_MESSAGE),
     callback_(callback),
     ts_lib_(ts_lib)
   {}
@@ -123,6 +123,31 @@ 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/node.hpp

@@ -56,6 +56,7 @@
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
 #include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/node_options.hpp"
 #include "rclcpp/parameter.hpp"
@@ -1454,6 +1455,11 @@ public:
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
   get_node_time_source_interface();
 
+  /// Return the Node's internal NodeTypeDescriptionsInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
+  get_node_type_descriptions_interface();
+
   /// Return the sub-namespace, if this is a sub-node, otherwise an empty string.
   /**
    * The returned sub-namespace is either the accumulated sub-namespaces which
@@ -1591,6 +1597,11 @@ private:
   const rclcpp::NodeOptions node_options_;
   const std::string sub_namespace_;
   const std::string effective_namespace_;
+
+  /// Static map(s) containing extra member variables for Node without changing its ABI.
+  // See node.cpp for more details.
+  class BackportMembers;
+  static BackportMembers backport_members_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -121,10 +121,19 @@ 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;
@@ -153,7 +162,7 @@ private:
 
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
-  rclcpp::GuardCondition notify_guard_condition_;
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -148,13 +148,33 @@ public:
   /**
    * For example, this should be notified when a publisher is added or removed.
    *
-   * \return the GuardCondition if it is valid, else thow runtime error
+   * \return the GuardCondition if it is valid, else throw 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,7 +57,8 @@ 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)
+    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile),
+    topic_type_hash_(info.topic_type_hash)
   {
     std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
   }
@@ -122,6 +123,16 @@ 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_;
@@ -129,6 +140,7 @@ 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/node_interfaces/node_interfaces.hpp

@@ -30,6 +30,7 @@
   rclcpp::node_interfaces::NodeTimeSourceInterface, \
   rclcpp::node_interfaces::NodeTimersInterface, \
   rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \
   rclcpp::node_interfaces::NodeWaitablesInterface
 
 
@@ -118,6 +119,7 @@ public:
    *   - rclcpp::node_interfaces::NodeTimeSourceInterface
    *   - rclcpp::node_interfaces::NodeTimersInterface
    *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeTypeDescriptionsInterface
    *   - rclcpp::node_interfaces::NodeWaitablesInterface
    *
    * Or you use custom interfaces as long as you make a template specialization

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -23,6 +23,9 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcl_interfaces/srv/get_logger_levels.hpp"
+#include "rcl_interfaces/srv/set_logger_levels.hpp"
+
 namespace rclcpp
 {
 namespace node_interfaces
@@ -35,7 +38,7 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
 
   RCLCPP_PUBLIC
-  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
 
   RCLCPP_PUBLIC
   virtual
@@ -49,13 +52,21 @@ public:
   const char *
   get_logger_name() const override;
 
+  RCLCPP_PUBLIC
+  void
+  create_logger_services(
+    node_interfaces::NodeServicesInterface::SharedPtr node_services) override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)
 
   /// Handle to the NodeBaseInterface given in the constructor.
-  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
 
   rclcpp::Logger logger_;
+
+  rclcpp::Service<rcl_interfaces::srv::GetLoggerLevels>::SharedPtr get_loggers_service_;
+  rclcpp::Service<rcl_interfaces::srv::SetLoggerLevels>::SharedPtr set_loggers_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -54,6 +55,13 @@ public:
   virtual
   const char *
   get_logger_name() const = 0;
+
+  /// create logger services
+  RCLCPP_PUBLIC
+  virtual
+  void
+  create_logger_services(
+    node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp

@@ -0,0 +1,63 @@
+// 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
+
+#include <memory>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTypeDescriptions part of the Node API.
+class NodeTypeDescriptions : public NodeTypeDescriptionsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions)
+
+  RCLCPP_PUBLIC
+  explicit NodeTypeDescriptions(
+    NodeBaseInterface::SharedPtr node_base,
+    NodeLoggingInterface::SharedPtr node_logging,
+    NodeParametersInterface::SharedPtr node_parameters,
+    NodeServicesInterface::SharedPtr node_services);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTypeDescriptions();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTypeDescriptions)
+
+  // Pimpl hides helper types and functions used for wrapping a C service, which would be
+  // awkward to expose in this header.
+  class NodeTypeDescriptionsImpl;
+  std::unique_ptr<NodeTypeDescriptionsImpl> impl_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp

@@ -0,0 +1,44 @@
+// 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API.
+class NodeTypeDescriptionsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTypeDescriptionsInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions)
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -43,6 +43,7 @@ public:
    *   - arguments = {}
    *   - parameter_overrides = {}
    *   - use_global_arguments = true
+   *   - enable_rosout = true
    *   - use_intra_process_comms = false
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
@@ -50,6 +51,7 @@ public:
    *   - clock_type = RCL_ROS_TIME
    *   - clock_qos = rclcpp::ClockQoS()
    *   - use_clock_thread = true
+   *   - enable_logger_service = false
    *   - rosout_qos = rclcpp::RosoutQoS()
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
    *     - with history setting and depth from rmw_qos_profile_parameter_events
@@ -232,6 +234,24 @@ public:
   NodeOptions &
   start_parameter_services(bool start_parameter_services);
 
+  /// Return the enable_logger_service flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_logger_service() const;
+
+  /// Set the enable_logger_service flag, return this for logger idiom.
+  /**
+   * If true, ROS services are created to allow external nodes to get
+   * and set logger levels of this node.
+   *
+   * If false, loggers will still be configured and set logger levels locally,
+   * but logger levels cannot be changed remotely .
+   *
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_logger_service(bool enable_log_service);
+
   /// Return the start_parameter_event_publisher flag.
   RCLCPP_PUBLIC
   bool
@@ -421,6 +441,8 @@ private:
 
   bool use_clock_thread_ {true};
 
+  bool enable_logger_service_ {false};
+
   rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
     rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
   );

rclcpp/include/rclcpp/publisher.hpp

@@ -484,6 +484,10 @@ 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_,
@@ -502,6 +506,10 @@ 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_,
@@ -521,6 +529,10 @@ 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,6 +117,18 @@
  * - 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/service.hpp

@@ -482,12 +482,20 @@ public:
   {
     rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
 
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        node_logger_.get_child("rclcpp"),
+        "failed to send response to %s (timeout): %s",
+        this->get_service_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (ret != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
     }
   }
 
-  /// Configure client introspection.
+  /// Configure service introspection.
   /**
    * \param[in] clock clock to use to generate introspection timestamps
    * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher

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()),
+      callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE),  // NOLINT
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
@@ -388,6 +388,57 @@ 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,6 +31,9 @@
 
 #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"
@@ -60,6 +63,27 @@ 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>
@@ -76,7 +100,8 @@ 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] is_serialized is true if the message will be delivered still serialized
+   * \param[in] delivered_message_kind Enum flag to change how the message will be received and
+   *            delivered
    */
   RCLCPP_PUBLIC
   SubscriptionBase(
@@ -86,7 +111,7 @@ public:
     const rcl_subscription_options_t & subscription_options,
     const SubscriptionEventCallbacks & event_callbacks,
     bool use_default_callbacks,
-    bool is_serialized = false);
+    DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE);
 
   /// Destructor.
   RCLCPP_PUBLIC
@@ -235,6 +260,14 @@ public:
   bool
   is_serialized() const;
 
+  /// Return the delivered message kind.
+  /**
+   * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
+   */
+  RCLCPP_PUBLIC
+  DeliveredMessageKind
+  get_delivered_message_kind() const;
+
   /// Get matching publisher count.
   /** \return The number of publishers on this topic. */
   RCLCPP_PUBLIC
@@ -535,6 +568,49 @@ 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
@@ -587,7 +663,7 @@ private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
   rosidl_message_type_support_t type_support_;
-  bool is_serialized_;
+  DeliveredMessageKind delivered_message_kind_;
 
   std::atomic<bool> subscription_in_use_by_wait_set_{false};
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};

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>19.3.0</version>
+  <version>21.0.7</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
@@ -39,6 +39,7 @@
   <depend>rcpputils</depend>
   <depend>rcutils</depend>
   <depend>rmw</depend>
+  <depend>rosidl_dynamic_typesupport</depend>
   <depend>statistics_msgs</depend>
   <depend>tracetools</depend>
 

rclcpp/src/rclcpp/callback_group.cpp

@@ -31,10 +31,12 @@ 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)
+  automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node),
+  get_context_(get_context)
 {}
 
 CallbackGroup::~CallbackGroup()
@@ -54,6 +56,16 @@ 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,
@@ -111,6 +123,7 @@ 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)
 {
@@ -129,6 +142,29 @@ 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/context.cpp

@@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
       std::unique_lock<std::mutex> lock(interrupt_mutex_);
       auto start = std::chrono::steady_clock::now();
       // this will release the lock while waiting
-      interrupt_condition_variable_.wait_for(lock, nanoseconds);
+      interrupt_condition_variable_.wait_for(lock, time_left);
       time_left -= std::chrono::steady_clock::now() - start;
     }
   } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp

@@ -0,0 +1,40 @@
+// 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

@@ -0,0 +1,38 @@
+// 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

@@ -0,0 +1,37 @@
+// 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

@@ -0,0 +1,49 @@
+// 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

@@ -0,0 +1,46 @@
+// 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/event_handler.cpp

@@ -39,13 +39,6 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
 
 EventHandlerBase::~EventHandlerBase()
 {
-  // Since the rmw event listener holds a reference to
-  // this callback, we need to clear it on destruction of this class.
-  // This clearing is not needed for other rclcpp entities like pub/subs, since
-  // they do own the underlying rmw entities, which are destroyed
-  // on their rclcpp destructors, thus no risk of dangling pointers.
-  clear_on_ready_callback();
-
   if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",

rclcpp/src/rclcpp/executor.cpp

@@ -24,6 +24,7 @@
 #include "rcl/error_handling.h"
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/guard_condition.hpp"
@@ -38,16 +39,16 @@
 using namespace std::chrono_literals;
 
 using rclcpp::exceptions::throw_from_rcl_error;
-using rclcpp::AnyExecutable;
 using rclcpp::Executor;
-using rclcpp::ExecutorOptions;
-using rclcpp::FutureReturnCode;
+
+class rclcpp::ExecutorImplementation {};
 
 Executor::Executor(const rclcpp::ExecutorOptions & options)
 : spinning(false),
-  interrupt_guard_condition_(options.context),
+  interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
-  memory_strategy_(options.memory_strategy)
+  memory_strategy_(options.memory_strategy),
+  impl_(std::make_unique<rclcpp::ExecutorImplementation>())
 {
   // Store the context for later use.
   context_ = options.context;
@@ -65,7 +66,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
   memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get());
 
   // Put the executor's guard condition in
-  memory_strategy_->add_guard_condition(interrupt_guard_condition_);
+  memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get());
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
   rcl_ret_t ret = rcl_wait_set_init(
@@ -127,7 +128,7 @@ Executor::~Executor()
   }
   // Remove and release the sigint guard condition
   memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get());
-  memory_strategy_->remove_guard_condition(&interrupt_guard_condition_);
+  memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get());
 
   // Remove shutdown callback handle registered to Context
   if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
@@ -222,8 +223,7 @@ Executor::add_callback_group_to_map(
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
 
   if (node_ptr->get_context()->is_valid()) {
-    auto callback_group_guard_condition =
-      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+    auto callback_group_guard_condition = group_ptr->get_notify_guard_condition();
     weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
     // Add the callback_group's notify condition to the guard condition handles
     memory_strategy_->add_guard_condition(*callback_group_guard_condition);
@@ -232,7 +232,7 @@ Executor::add_callback_group_to_map(
   if (notify) {
     // Interrupt waiting to handle new node
     try {
-      interrupt_guard_condition_.trigger();
+      interrupt_guard_condition_->trigger();
     } catch (const rclcpp::exceptions::RCLError & ex) {
       throw std::runtime_error(
               std::string(
@@ -280,10 +280,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
       }
     });
 
-  const auto & gc = node_ptr->get_notify_guard_condition();
-  weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+  const auto gc = node_ptr->get_shared_notify_guard_condition();
+  weak_nodes_to_guard_conditions_[node_ptr] = gc.get();
   // Add the node's notify condition to the guard condition handles
-  memory_strategy_->add_guard_condition(gc);
+  memory_strategy_->add_guard_condition(*gc);
   weak_nodes_.push_back(node_ptr);
 }
 
@@ -320,7 +320,7 @@ Executor::remove_callback_group_from_map(
 
     if (notify) {
       try {
-        interrupt_guard_condition_.trigger();
+        interrupt_guard_condition_->trigger();
       } catch (const rclcpp::exceptions::RCLError & ex) {
         throw std::runtime_error(
                 std::string(
@@ -388,7 +388,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     }
   }
 
-  memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
+  memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get());
   weak_nodes_to_guard_conditions_.erase(node_ptr);
 
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
@@ -501,7 +501,7 @@ Executor::cancel()
 {
   spinning.store(false);
   try {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("Failed to trigger guard condition in cancel: ") + ex.what());
@@ -550,7 +550,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   try {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(
@@ -598,70 +598,103 @@ take_and_do_error_handling(
 void
 Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
 {
+  using rclcpp::dynamic_typesupport::DynamicMessage;
+
   rclcpp::MessageInfo message_info;
   message_info.get_rmw_message_info().from_intra_process = false;
 
-  if (subscription->is_serialized()) {
-    // This is the case where a copy of the serialized message is taken from
-    // the middleware via inter-process communication.
-    std::shared_ptr<SerializedMessage> serialized_msg = subscription->create_serialized_message();
-    take_and_do_error_handling(
-      "taking a serialized message from topic",
-      subscription->get_topic_name(),
-      [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
-      [&]()
+  switch (subscription->get_delivered_message_kind()) {
+    // Deliver ROS message
+    case rclcpp::DeliveredMessageKind::ROS_MESSAGE:
       {
-        subscription->handle_serialized_message(serialized_msg, message_info);
-      });
-    subscription->return_serialized_message(serialized_msg);
-  } else if (subscription->can_loan_messages()) {
-    // This is the case where a loaned message is taken from the middleware via
-    // inter-process communication, given to the user for their callback,
-    // and then returned.
-    void * loaned_msg = nullptr;
-    // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
-    //   is extened to support subscriptions as well.
-    take_and_do_error_handling(
-      "taking a loaned message from topic",
-      subscription->get_topic_name(),
-      [&]()
-      {
-        rcl_ret_t ret = rcl_take_loaned_message(
-          subscription->get_subscription_handle().get(),
-          &loaned_msg,
-          &message_info.get_rmw_message_info(),
-          nullptr);
-        if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
-          return false;
-        } else if (RCL_RET_OK != ret) {
-          rclcpp::exceptions::throw_from_rcl_error(ret);
+        if (subscription->can_loan_messages()) {
+          // This is the case where a loaned message is taken from the middleware via
+          // inter-process communication, given to the user for their callback,
+          // and then returned.
+          void * loaned_msg = nullptr;
+          // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
+          //   is extened to support subscriptions as well.
+          take_and_do_error_handling(
+            "taking a loaned message from topic",
+            subscription->get_topic_name(),
+            [&]()
+            {
+              rcl_ret_t ret = rcl_take_loaned_message(
+                subscription->get_subscription_handle().get(),
+                &loaned_msg,
+                &message_info.get_rmw_message_info(),
+                nullptr);
+              if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
+                return false;
+              } else if (RCL_RET_OK != ret) {
+                rclcpp::exceptions::throw_from_rcl_error(ret);
+              }
+              return true;
+            },
+            [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
+          if (nullptr != loaned_msg) {
+            rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
+              subscription->get_subscription_handle().get(), loaned_msg);
+            if (RCL_RET_OK != ret) {
+              RCLCPP_ERROR(
+                rclcpp::get_logger("rclcpp"),
+                "rcl_return_loaned_message_from_subscription() failed for subscription on topic "
+                "'%s': %s",
+                subscription->get_topic_name(), rcl_get_error_string().str);
+            }
+            loaned_msg = nullptr;
+          }
+        } else {
+          // This case is taking a copy of the message data from the middleware via
+          // inter-process communication.
+          std::shared_ptr<void> message = subscription->create_message();
+          take_and_do_error_handling(
+            "taking a message from topic",
+            subscription->get_topic_name(),
+            [&]() {return subscription->take_type_erased(message.get(), message_info);},
+            [&]() {subscription->handle_message(message, message_info);});
+          // TODO(clalancette): In the case that the user is using the MessageMemoryPool,
+          // and they take a shared_ptr reference to the message in the callback, this can
+          // inadvertently return the message to the pool when the user is still using it.
+          // This is a bug that needs to be fixed in the pool, and we should probably have
+          // a custom deleter for the message that actually does the return_message().
+          subscription->return_message(message);
         }
-        return true;
-      },
-      [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
-    if (nullptr != loaned_msg) {
-      rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
-        subscription->get_subscription_handle().get(),
-        loaned_msg);
-      if (RCL_RET_OK != ret) {
-        RCLCPP_ERROR(
-          rclcpp::get_logger("rclcpp"),
-          "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s",
-          subscription->get_topic_name(), rcl_get_error_string().str);
+        break;
+      }
+
+    // Deliver serialized message
+    case rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE:
+      {
+        // This is the case where a copy of the serialized message is taken from
+        // the middleware via inter-process communication.
+        std::shared_ptr<SerializedMessage> serialized_msg =
+          subscription->create_serialized_message();
+        take_and_do_error_handling(
+          "taking a serialized message from topic",
+          subscription->get_topic_name(),
+          [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
+          [&]()
+          {
+            subscription->handle_serialized_message(serialized_msg, message_info);
+          });
+        subscription->return_serialized_message(serialized_msg);
+        break;
+      }
+
+    // DYNAMIC SUBSCRIPTION ========================================================================
+    // Deliver dynamic message
+    case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE:
+      {
+        throw std::runtime_error("Unimplemented");
+      }
+
+    default:
+      {
+        throw std::runtime_error("Delivered message kind is not supported");
       }
-      loaned_msg = nullptr;
-    }
-  } else {
-    // This case is taking a copy of the message data from the middleware via
-    // inter-process communication.
-    std::shared_ptr<void> message = subscription->create_message();
-    take_and_do_error_handling(
-      "taking a message from topic",
-      subscription->get_topic_name(),
-      [&]() {return subscription->take_type_erased(message.get(), message_info);},
-      [&]() {subscription->handle_message(message, message_info);});
-    subscription->return_message(message);
   }
+  return;
 }
 
 void

rclcpp/src/rclcpp/executors/executor_entities_collection.cpp

@@ -0,0 +1,230 @@
+// 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

@@ -0,0 +1,416 @@
+// 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)
+{
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    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 {
+      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

@@ -0,0 +1,183 @@
+// 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) {
+      auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition();
+
+      rcl_ret_t ret = rcl_wait_set_add_guard_condition(
+        wait_set,
+        rcl_guard_condition, 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/static_executor_entities_collector.cpp

@@ -109,8 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
   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;
+      weak_nodes_to_guard_conditions_[node_ptr] =
+        node_ptr->get_shared_notify_guard_condition().get();
     }
   }
   new_nodes_.clear();

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -139,7 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group(
   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();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -150,7 +150,7 @@ StaticSingleThreadedExecutor::add_node(
   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();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -167,7 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group(
   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();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -181,7 +181,7 @@ StaticSingleThreadedExecutor::remove_node(
   }
   // If the node was matched and removed, interrupt waiting
   if (notify) {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   }
 }
 

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

@@ -0,0 +1,495 @@
+// 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.hpp"
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "rcpputils/scope_exit.hpp"
+
+using namespace std::chrono_literals;
+
+using rclcpp::experimental::executors::EventsExecutor;
+
+EventsExecutor::EventsExecutor(
+  rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue,
+  bool execute_timers_separate_thread,
+  const rclcpp::ExecutorOptions & options)
+: rclcpp::Executor(options)
+{
+  // Get ownership of the queue used to store events.
+  if (!events_queue) {
+    throw std::invalid_argument("events_queue can't be a null pointer");
+  }
+  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) {
+        ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
+        this->events_queue_->enqueue(event);
+      };
+  }
+  timers_manager_ =
+    std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
+
+  this->current_entities_collection_ =
+    std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
+
+  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();
+    });
+
+  // Make sure that the notify waitable is immediately added to the collection
+  // to avoid missing events
+  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
+
+  notify_waitable_->add_guard_condition(interrupt_guard_condition_);
+  notify_waitable_->add_guard_condition(shutdown_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_);
+}
+
+EventsExecutor::~EventsExecutor()
+{
+  spinning.store(false);
+  notify_waitable_->clear_on_ready_callback();
+  this->refresh_current_collection({});
+}
+
+void
+EventsExecutor::spin()
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+
+  timers_manager_->start();
+  RCPPUTILS_SCOPE_EXIT(timers_manager_->stop(); );
+
+  while (rclcpp::ok(context_) && spinning.load()) {
+    // Wait until we get an event
+    ExecutorEvent event;
+    bool has_event = events_queue_->dequeue(event);
+    if (has_event) {
+      this->execute_event(event);
+    }
+  }
+}
+
+void
+EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
+{
+  return this->spin_some_impl(max_duration, false);
+}
+
+void
+EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
+{
+  if (max_duration <= 0ns) {
+    throw std::invalid_argument("max_duration must be positive");
+  }
+  return this->spin_some_impl(max_duration, true);
+}
+
+void
+EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
+{
+  if (spinning.exchange(true)) {
+    throw std::runtime_error("spin_some() called while already spinning");
+  }
+
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+
+  auto start = std::chrono::steady_clock::now();
+
+  auto max_duration_not_elapsed = [max_duration, start]() {
+      if (std::chrono::nanoseconds(0) == max_duration) {
+        // told to spin forever if need be
+        return true;
+      } else if (std::chrono::steady_clock::now() - start < max_duration) {
+        // told to spin only for some maximum amount of time
+        return true;
+      }
+      // spun too long
+      return false;
+    };
+
+  // Get the number of events and timers ready at start
+  const size_t ready_events_at_start = events_queue_->size();
+  size_t executed_events = 0;
+  const size_t ready_timers_at_start = timers_manager_->get_number_ready_timers();
+  size_t executed_timers = 0;
+
+  while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
+    // Execute first ready event from queue if exists
+    if (exhaustive || (executed_events < ready_events_at_start)) {
+      bool has_event = !events_queue_->empty();
+
+      if (has_event) {
+        ExecutorEvent event;
+        bool ret = events_queue_->dequeue(event, std::chrono::nanoseconds(0));
+        if (ret) {
+          this->execute_event(event);
+          executed_events++;
+          continue;
+        }
+      }
+    }
+
+    // Execute first timer if it is ready
+    if (exhaustive || (executed_timers < ready_timers_at_start)) {
+      bool timer_executed = timers_manager_->execute_head_timer();
+      if (timer_executed) {
+        executed_timers++;
+        continue;
+      }
+    }
+
+    // If there's no more work available, exit
+    break;
+  }
+}
+
+void
+EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
+{
+  // In this context a negative input timeout means no timeout
+  if (timeout < 0ns) {
+    timeout = std::chrono::nanoseconds::max();
+  }
+
+  // Select the smallest between input timeout and timer timeout
+  bool is_timer_timeout = false;
+  auto next_timer_timeout = timers_manager_->get_head_timeout();
+  if (next_timer_timeout < timeout) {
+    timeout = next_timer_timeout;
+    is_timer_timeout = true;
+  }
+
+  ExecutorEvent event;
+  bool has_event = events_queue_->dequeue(event, timeout);
+
+  // If we wake up from the wait with an event, it means that it
+  // arrived before any of the timers expired.
+  if (has_event) {
+    this->execute_event(event);
+  } else if (is_timer_timeout) {
+    timers_manager_->execute_head_timer();
+  }
+}
+
+void
+EventsExecutor::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  // This field is unused because we don't have to wake up the executor when a node is added.
+  (void) notify;
+
+  // Add node to entities collector
+  this->entities_collector_->add_node(node_ptr);
+
+  this->refresh_current_collection_from_callback_groups();
+}
+
+void
+EventsExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->add_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+EventsExecutor::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
+{
+  // This field is unused because we don't have to wake up the executor when a node is removed.
+  (void)notify;
+
+  // Remove node from entities collector.
+  // 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();
+}
+
+void
+EventsExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
+{
+  this->remove_node(node_ptr->get_node_base_interface(), notify);
+}
+
+void
+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);
+
+        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);
+        if (subscription) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_subscription(subscription);
+          }
+        }
+        break;
+      }
+    case ExecutorEventType::SERVICE_EVENT:
+      {
+        auto service = this->retrieve_entity(
+          static_cast<const rcl_service_t *>(event.entity_key),
+          current_entities_collection_->services);
+
+        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));
+        break;
+      }
+    case ExecutorEventType::WAITABLE_EVENT:
+      {
+        auto waitable = this->retrieve_entity(
+          static_cast<const rclcpp::Waitable *>(event.entity_key),
+          current_entities_collection_->waitables);
+        if (waitable) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            auto data = waitable->take_data_by_entity_id(event.waitable_data);
+            waitable->execute(data);
+          }
+        }
+        break;
+      }
+  }
+}
+
+void
+EventsExecutor::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  bool notify)
+{
+  // 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();
+}
+
+void
+EventsExecutor::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
+{
+  // 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();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutor::get_all_callback_groups()
+{
+  this->entities_collector_->update_collections();
+  return this->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();
+}
+
+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.
+  this->add_notify_waitable_to_collection(new_collection.waitables);
+  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
+
+  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;
+}
+
+void
+EventsExecutor::add_notify_waitable_to_collection(
+  rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection)
+{
+  // The notify waitable is not associated to any group, so use an invalid one
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
+  collection.insert(
+  {
+    this->notify_waitable_.get(),
+    {this->notify_waitable_, weak_group_ptr}
+  });
+}

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -0,0 +1,299 @@
+// 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/timers_manager.hpp"
+
+#include <inttypes.h>
+
+#include <ctime>
+#include <iostream>
+#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)
+{
+  context_ = context;
+  on_ready_callback_ = on_ready_callback;
+}
+
+TimersManager::~TimersManager()
+{
+  // Remove all timers
+  this->clear();
+
+  // Make sure timers thread is stopped before destroying this object
+  this->stop();
+}
+
+void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer)
+{
+  if (!timer) {
+    throw std::invalid_argument("TimersManager::add_timer() trying to add nullptr timer");
+  }
+
+  bool added = false;
+  {
+    std::unique_lock<std::mutex> lock(timers_mutex_);
+    added = weak_timers_heap_.add_timer(timer);
+    timers_updated_ = timers_updated_ || added;
+  }
+
+  timer->set_on_reset_callback(
+    [this](size_t arg) {
+      {
+        (void)arg;
+        std::unique_lock<std::mutex> lock(timers_mutex_);
+        timers_updated_ = true;
+      }
+      timers_cv_.notify_one();
+    });
+
+  if (added) {
+    // Notify that a timer has been added
+    timers_cv_.notify_one();
+  }
+}
+
+void TimersManager::start()
+{
+  // Make sure that the thread is not already running
+  if (running_.exchange(true)) {
+    throw std::runtime_error("TimersManager::start() can't start timers thread as already running");
+  }
+
+  timers_thread_ = std::thread(&TimersManager::run_timers, this);
+}
+
+void TimersManager::stop()
+{
+  // Lock stop() function to prevent race condition in destructor
+  std::unique_lock<std::mutex> lock(stop_mutex_);
+  running_ = false;
+
+  // Notify the timers manager thread to wake up
+  {
+    std::unique_lock<std::mutex> lock(timers_mutex_);
+    timers_updated_ = true;
+  }
+  timers_cv_.notify_one();
+
+  // Join timers thread if it's running
+  if (timers_thread_.joinable()) {
+    timers_thread_.join();
+  }
+}
+
+std::chrono::nanoseconds TimersManager::get_head_timeout()
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "get_head_timeout() can't be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+  return this->get_head_timeout_unsafe();
+}
+
+size_t TimersManager::get_number_ready_timers()
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "get_number_ready_timers() can't be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+  TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+  return locked_heap.get_number_ready_timers();
+}
+
+bool TimersManager::execute_head_timer()
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "execute_head_timer() can't be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+
+  TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
+
+  // Nothing to do if we don't have any timer
+  if (timers_heap.empty()) {
+    return false;
+  }
+
+  TimerPtr head_timer = timers_heap.front();
+
+  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();
+    timers_heap.heapify_root();
+    weak_timers_heap_.store(timers_heap);
+  }
+
+  return timer_ready;
+}
+
+void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
+{
+  TimerPtr ready_timer;
+  {
+    std::unique_lock<std::mutex> lock(timers_mutex_);
+    ready_timer = weak_timers_heap_.get_timer(timer_id);
+  }
+  if (ready_timer) {
+    ready_timer->execute_callback();
+  }
+}
+
+std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
+{
+  // If we don't have any weak pointer, then we just return maximum timeout
+  if (weak_timers_heap_.empty()) {
+    return std::chrono::nanoseconds::max();
+  }
+  // Weak heap is not empty, so try to lock the first element.
+  // If it is still a valid pointer, it is guaranteed to be the correct head
+  TimerPtr head_timer = weak_timers_heap_.front().lock();
+
+  if (!head_timer) {
+    // The first element has expired, we can't make other assumptions on the heap
+    // and we need to entirely validate it.
+    TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+    // NOTE: the following operations will not modify any element in the heap, so we
+    // don't have to call `weak_timers_heap_.store(locked_heap)` at the end.
+
+    if (locked_heap.empty()) {
+      return std::chrono::nanoseconds::max();
+    }
+    head_timer = locked_heap.front();
+  }
+
+  return head_timer->time_until_trigger();
+}
+
+void TimersManager::execute_ready_timers_unsafe()
+{
+  // We start by locking the timers
+  TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+
+  // Nothing to do if we don't have any timer
+  if (locked_heap.empty()) {
+    return;
+  }
+
+  // Keep executing timers until they are ready and they were already ready when we started.
+  // The two checks prevent this function from blocking indefinitely if the
+  // time required for executing the timers is longer than their period.
+
+  TimerPtr head_timer = locked_heap.front();
+  const size_t number_ready_timers = locked_heap.get_number_ready_timers();
+  size_t executed_timers = 0;
+  while (executed_timers < number_ready_timers && head_timer->is_ready()) {
+    head_timer->call();
+    if (on_ready_callback_) {
+      on_ready_callback_(head_timer.get());
+    } else {
+      head_timer->execute_callback();
+    }
+
+    executed_timers++;
+    // Executing a timer will result in updating its time_until_trigger, so re-heapify
+    locked_heap.heapify_root();
+    // Get new head timer
+    head_timer = locked_heap.front();
+  }
+
+  // After having performed work on the locked heap we reflect the changes to weak one.
+  // Timers will be already sorted the next time we need them if none went out of scope.
+  weak_timers_heap_.store(locked_heap);
+}
+
+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_);
+
+    std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
+
+    // No need to wait if a timer is already available
+    if (time_to_sleep > std::chrono::nanoseconds::zero()) {
+      if (time_to_sleep != std::chrono::nanoseconds::max()) {
+        // Wait until timeout or notification that timers have been updated
+        timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
+      } else {
+        // Wait until notification that timers have been updated
+        timers_cv_.wait(lock, [this]() {return timers_updated_;});
+      }
+    }
+
+    // Reset timers updated flag
+    timers_updated_ = false;
+
+    // Execute timers
+    this->execute_ready_timers_unsafe();
+  }
+}
+
+void TimersManager::clear()
+{
+  {
+    // Lock mutex and then clear all data structures
+    std::unique_lock<std::mutex> lock(timers_mutex_);
+
+    TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
+    locked_heap.clear_timers_on_reset_callbacks();
+
+    weak_timers_heap_.clear();
+
+    timers_updated_ = true;
+  }
+
+  // Notify timers thread such that it can re-compute its timeout
+  timers_cv_.notify_one();
+}
+
+void TimersManager::remove_timer(TimerPtr timer)
+{
+  bool removed = false;
+  {
+    std::unique_lock<std::mutex> lock(timers_mutex_);
+    removed = weak_timers_heap_.remove_timer(timer);
+
+    timers_updated_ = timers_updated_ || removed;
+  }
+
+  if (removed) {
+    // Notify timers thread such that it can re-compute its timeout
+    timers_cv_.notify_one();
+    timer->clear_on_reset_callback();
+  }
+}

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -25,17 +25,20 @@
 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 &)
 {
@@ -51,7 +54,8 @@ 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;
@@ -60,16 +64,69 @@ void 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/logging_mutex.cpp

@@ -14,6 +14,7 @@
 
 #include <memory>
 #include <mutex>
+#include <stdexcept>
 
 #include "rcutils/macros.h"
 

rclcpp/src/rclcpp/node.cpp

@@ -17,7 +17,10 @@
 #include <limits>
 #include <map>
 #include <memory>
+#include <mutex>
+#include <shared_mutex>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -36,6 +39,7 @@
 #include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
 
@@ -109,6 +113,72 @@ create_effective_namespace(const std::string & node_namespace, const std::string
 
 }  // namespace
 
+/// \brief Associate new extra member variables with instances of Node without changing ABI.
+/**
+ * It is used only for bugfixes or backported features that require new members.
+ * Atomically constructs/destroys all extra members.
+ * Node instance will register and remove itself, and use its methods to retrieve members.
+ * Note for performance consideration that accessing these members uses a map lookup.
+ */
+class Node::BackportMembers
+{
+public:
+  BackportMembers() = default;
+  ~BackportMembers() = default;
+
+  /// \brief Add all backported members for a new Node.
+  /**
+   * \param[in] key Raw pointer to the Node instance that will use new members.
+   */
+  void add(Node * key)
+  {
+    // Adding a new instance to the maps requires exclusive access
+    std::unique_lock lock(map_access_mutex_);
+    type_descriptions_map_.emplace(
+      key,
+      std::make_shared<rclcpp::node_interfaces::NodeTypeDescriptions>(
+        key->get_node_base_interface(),
+        key->get_node_logging_interface(),
+        key->get_node_parameters_interface(),
+        key->get_node_services_interface()));
+  }
+
+  /// \brief Remove the members for an instance of Node
+  /**
+   * \param[in] key Raw pointer to the Node
+   */
+  void remove(const Node * key)
+  {
+    // Removing an instance from the maps requires exclusive access
+    std::unique_lock lock(map_access_mutex_);
+    type_descriptions_map_.erase(key);
+  }
+
+  /// \brief Retrieve the NodeTypeDescriptionsInterface for a Node.
+  /**
+   * \param[in] key Raw pointer to an instance of Node.
+   * \return A shared ptr to this Node's NodeTypeDescriptionsInterface instance.
+   */
+  rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
+  get_node_type_descriptions_interface(const Node * key) const
+  {
+    // Multiple threads can retrieve from the maps at the same time
+    std::shared_lock lock(map_access_mutex_);
+    return type_descriptions_map_.at(key);
+  }
+
+private:
+  /// \brief Map that stored TypeDescriptionsInterface members
+  std::unordered_map<
+    const Node *, rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
+  > type_descriptions_map_;
+
+  /// \brief Controls access to all private maps
+  mutable std::shared_mutex map_access_mutex_;
+};
+// Definition of static member declaration
+Node::BackportMembers Node::backport_members_;
+
 Node::Node(
   const std::string & node_name,
   const NodeOptions & options)
@@ -167,7 +237,7 @@ Node::Node(
       options.use_intra_process_comms(),
       options.enable_topic_statistics())),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
-  node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
+  node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)),
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
@@ -211,6 +281,8 @@ Node::Node(
   sub_namespace_(""),
   effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
+  backport_members_.add(this);
+
   // we have got what we wanted directly from the overrides,
   // but declare the parameters anyway so they are visible.
   rclcpp::detail::declare_qos_parameters(
@@ -225,6 +297,10 @@ Node::Node(
     node_topics_->resolve_topic_name("/parameter_events"),
     options.parameter_event_qos(),
     rclcpp::detail::PublisherQosParametersTraits{});
+
+  if (options.enable_logger_service()) {
+    node_logging_->create_logger_services(node_services_);
+  }
 }
 
 Node::Node(
@@ -268,6 +344,7 @@ Node::Node(
 Node::~Node()
 {
   // release sub-interfaces in an order that allows them to consult with node_base during tear-down
+  backport_members_.remove(this);
   node_waitables_.reset();
   node_time_source_.reset();
   node_parameters_.reset();
@@ -587,6 +664,12 @@ Node::get_node_topics_interface()
   return node_topics_;
 }
 
+rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
+Node::get_node_type_descriptions_interface()
+{
+  return backport_members_.get_node_type_descriptions_interface(this);
+}
+
 rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
 Node::get_node_services_interface()
 {

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_(context),
+  notify_guard_condition_(std::make_shared<rclcpp::GuardCondition>(context)),
   notify_guard_condition_is_valid_(false)
 {
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
@@ -132,8 +132,10 @@ 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);
+      NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true);
   }
 
   // Indicate the notify_guard_condition is now valid.
@@ -202,11 +204,27 @@ 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;
 }
 
@@ -253,9 +271,29 @@ 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,9 +533,8 @@ NodeGraph::notify_graph_change()
     }
   }
   graph_cv_.notify_all();
-  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on graph change: ") + ex.what());
@@ -789,3 +788,15 @@ 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_logging.cpp

@@ -12,11 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "rclcpp/node_impl.hpp"
 #include "rclcpp/node_interfaces/node_logging.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
 
 using rclcpp::node_interfaces::NodeLogging;
 
-NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
 : node_base_(node_base)
 {
   logger_ = rclcpp::get_logger(NodeLogging::get_logger_name());
@@ -37,3 +39,55 @@ NodeLogging::get_logger_name() const
 {
   return rcl_node_get_logger_name(node_base_->get_rcl_node_handle());
 }
+
+void NodeLogging::create_logger_services(
+  node_interfaces::NodeServicesInterface::SharedPtr node_services)
+{
+  rclcpp::ServicesQoS qos_profile;
+  const std::string node_name = node_base_->get_name();
+  auto callback_group = node_base_->get_default_callback_group();
+
+  get_loggers_service_ = rclcpp::create_service<rcl_interfaces::srv::GetLoggerLevels>(
+    node_base_, node_services,
+    node_name + "/get_logger_levels",
+    [](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::GetLoggerLevels::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::GetLoggerLevels::Response> response)
+    {
+      for (auto & name : request->names) {
+        rcl_interfaces::msg::LoggerLevel logger_level;
+        logger_level.name = name;
+        auto ret = rcutils_logging_get_logger_level(name.c_str());
+        if (ret < 0) {
+          logger_level.level = 0;
+        } else {
+          logger_level.level = static_cast<uint8_t>(ret);
+        }
+        response->levels.push_back(std::move(logger_level));
+      }
+    },
+    qos_profile, callback_group);
+
+  set_loggers_service_ = rclcpp::create_service<rcl_interfaces::srv::SetLoggerLevels>(
+    node_base_, node_services,
+    node_name + "/set_logger_levels",
+    [](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::SetLoggerLevels::Request> request,
+      std::shared_ptr<rcl_interfaces::srv::SetLoggerLevels::Response> response)
+    {
+      rcl_interfaces::msg::SetLoggerLevelsResult result;
+      for (auto & level : request->levels) {
+        auto ret = rcutils_logging_set_logger_level(level.name.c_str(), level.level);
+        if (ret != RCUTILS_RET_OK) {
+          result.successful = false;
+          result.reason = rcutils_get_error_string().str;
+        } else {
+          result.successful = true;
+        }
+        response->results.push_back(std::move(result));
+      }
+    },
+    qos_profile, callback_group);
+}

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -42,9 +42,8 @@ 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_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
@@ -69,9 +68,8 @@ 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_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -42,9 +42,8 @@ NodeTimers::add_timer(
   }
   callback_group->add_timer(timer);
 
-  auto & node_gc = node_base_->get_notify_guard_condition();
   try {
-    node_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     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,9 +70,8 @@ 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_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
@@ -119,9 +118,8 @@ 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_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp

@@ -0,0 +1,157 @@
+// 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 <memory>
+#include <string>
+#include <thread>
+
+#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
+#include "rclcpp/parameter_client.hpp"
+
+#include "type_description_interfaces/srv/get_type_description.h"
+
+namespace
+{
+// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation.
+struct GetTypeDescription__C
+{
+  using Request = type_description_interfaces__srv__GetTypeDescription_Request;
+  using Response = type_description_interfaces__srv__GetTypeDescription_Response;
+  using Event = type_description_interfaces__srv__GetTypeDescription_Event;
+};
+}  // namespace
+
+// Helper function for C typesupport.
+namespace rosidl_typesupport_cpp
+{
+template<>
+rosidl_service_type_support_t const *
+get_service_type_support_handle<GetTypeDescription__C>()
+{
+  return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription);
+}
+}  // namespace rosidl_typesupport_cpp
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+class NodeTypeDescriptions::NodeTypeDescriptionsImpl
+{
+public:
+  using ServiceT = GetTypeDescription__C;
+
+  rclcpp::Logger logger_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::Service<ServiceT>::SharedPtr type_description_srv_;
+
+  NodeTypeDescriptionsImpl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services)
+  : logger_(node_logging->get_logger()),
+    node_base_(node_base)
+  {
+    const std::string enable_param_name = "start_type_description_service";
+
+    bool enabled = false;
+    try {
+      auto enable_param = node_parameters->declare_parameter(
+        enable_param_name,
+        rclcpp::ParameterValue(true),
+        rcl_interfaces::msg::ParameterDescriptor()
+        .set__name(enable_param_name)
+        .set__type(rclcpp::PARAMETER_BOOL)
+        .set__description("Start the ~/get_type_description service for this node.")
+        .set__read_only(true));
+      enabled = enable_param.get<bool>();
+    } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) {
+      RCLCPP_ERROR(logger_, "%s", exc.what());
+      throw;
+    }
+
+    if (enabled) {
+      auto rcl_node = node_base->get_rcl_node_handle();
+      rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node);
+      if (rcl_ret != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          logger_, "Failed to initialize ~/get_type_description_service: %s",
+          rcl_get_error_string().str);
+        throw std::runtime_error(
+                "Failed to initialize ~/get_type_description service.");
+      }
+
+      rcl_service_t * rcl_srv = nullptr;
+      rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv);
+      if (rcl_ret != RCL_RET_OK) {
+        throw std::runtime_error(
+                "Failed to get initialized ~/get_type_description service from rcl.");
+      }
+
+      rclcpp::AnyServiceCallback<ServiceT> cb;
+      cb.set(
+        [this](
+          std::shared_ptr<rmw_request_id_t> header,
+          std::shared_ptr<ServiceT::Request> request,
+          std::shared_ptr<ServiceT::Response> response
+        ) {
+          rcl_node_type_description_service_handle_request(
+            node_base_->get_rcl_node_handle(),
+            header.get(),
+            request.get(),
+            response.get());
+        });
+
+      type_description_srv_ = std::make_shared<Service<ServiceT>>(
+        node_base_->get_shared_rcl_node_handle(),
+        rcl_srv,
+        cb);
+      node_services->add_service(
+        std::dynamic_pointer_cast<ServiceBase>(type_description_srv_),
+        nullptr);
+    }
+  }
+
+  ~NodeTypeDescriptionsImpl()
+  {
+    if (
+      type_description_srv_ &&
+      RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle()))
+    {
+      RCLCPP_ERROR(
+        logger_,
+        "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str);
+    }
+  }
+};
+
+NodeTypeDescriptions::NodeTypeDescriptions(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services)
+: impl_(new NodeTypeDescriptionsImpl(
+      node_base,
+      node_logging,
+      node_parameters,
+      node_services))
+{}
+
+NodeTypeDescriptions::~NodeTypeDescriptions()
+{}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -42,9 +42,8 @@ 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_gc.trigger();
+    node_base_->trigger_notify_guard_condition();
     group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(

rclcpp/src/rclcpp/node_options.cpp

@@ -248,6 +248,19 @@ NodeOptions::start_parameter_services(bool start_parameter_services)
   return *this;
 }
 
+bool
+NodeOptions::enable_logger_service() const
+{
+  return this->enable_logger_service_;
+}
+
+NodeOptions &
+NodeOptions::enable_logger_service(bool enable_logger_service)
+{
+  this->enable_logger_service_ = enable_logger_service;
+  return *this;
+}
+
 bool
 NodeOptions::start_parameter_event_publisher() const
 {

rclcpp/src/rclcpp/signal_handler.cpp

@@ -113,7 +113,7 @@ SignalHandler::get_logger()
 SignalHandler &
 SignalHandler::get_global_signal_handler()
 {
-  static SignalHandler signal_handler;
+  static SignalHandler & signal_handler = *new SignalHandler();
   return signal_handler;
 }
 

rclcpp/src/rclcpp/subscription_base.cpp

@@ -22,6 +22,7 @@
 
 #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"
@@ -32,6 +33,8 @@
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
+#include "rosidl_dynamic_typesupport/types.h"
+
 using rclcpp::SubscriptionBase;
 
 SubscriptionBase::SubscriptionBase(
@@ -41,7 +44,7 @@ SubscriptionBase::SubscriptionBase(
   const rcl_subscription_options_t & subscription_options,
   const SubscriptionEventCallbacks & event_callbacks,
   bool use_default_callbacks,
-  bool is_serialized)
+  DeliveredMessageKind delivered_message_kind)
 : node_base_(node_base),
   node_handle_(node_base_->get_shared_rcl_node_handle()),
   node_logger_(rclcpp::get_node_logger(node_handle_.get())),
@@ -49,7 +52,7 @@ SubscriptionBase::SubscriptionBase(
   intra_process_subscription_id_(0),
   event_callbacks_(event_callbacks),
   type_support_(type_support_handle),
-  is_serialized_(is_serialized)
+  delivered_message_kind_(delivered_message_kind)
 {
   auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs)
     {
@@ -258,7 +261,13 @@ SubscriptionBase::get_message_type_support_handle() const
 bool
 SubscriptionBase::is_serialized() const
 {
-  return is_serialized_;
+  return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE;
+}
+
+rclcpp::DeliveredMessageKind
+SubscriptionBase::get_delivered_message_kind() const
+{
+  return delivered_message_kind_;
 }
 
 size_t
@@ -289,7 +298,20 @@ SubscriptionBase::setup_intra_process(
 bool
 SubscriptionBase::can_loan_messages() const
 {
-  return rcl_subscription_can_loan_messages(subscription_handle_.get());
+  bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get());
+  if (retval) {
+    // TODO(clalancette): The loaned message interface is currently not safe to use with
+    // shared_ptr callbacks.  If a user takes a copy of the shared_ptr, it can get freed from
+    // underneath them via rcl_return_loaned_message_from_subscription().  The correct solution is
+    // to return the loaned message in a custom deleter, but that needs to be carefully handled
+    // with locking.  Warn the user about this until we fix it.
+    RCLCPP_WARN_ONCE(
+      this->node_logger_,
+      "Loaned messages are only safe with const ref subscription callbacks. "
+      "If you are using any other kind of subscriptions, "
+      "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default).");
+  }
+  return retval;
 }
 
 rclcpp::Waitable::SharedPtr
@@ -442,8 +464,7 @@ 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),
@@ -515,3 +536,14 @@ 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/rclcpp/CMakeLists.txt

@@ -262,6 +262,11 @@ if(TARGET test_node_interfaces__node_topics)
     "test_msgs")
   target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__node_type_descriptions
+  node_interfaces/test_node_type_descriptions.cpp)
+if(TARGET test_node_interfaces__node_type_descriptions)
+  target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME} mimick)
+endif()
 ament_add_gtest(test_node_interfaces__node_waitables
   node_interfaces/test_node_waitables.cpp)
 if(TARGET test_node_interfaces__node_waitables)
@@ -618,6 +623,14 @@ if(TARGET test_timer)
   target_link_libraries(test_timer ${PROJECT_NAME} mimick)
 endif()
 
+ament_add_gtest(test_timers_manager test_timers_manager.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_timers_manager)
+  ament_target_dependencies(test_timers_manager
+    "rcl")
+  target_link_libraries(test_timers_manager ${PROJECT_NAME} mimick)
+endif()
+
 ament_add_gtest(test_time_source test_time_source.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_time_source)
@@ -641,6 +654,13 @@ if(TARGET test_wait_for_message)
   target_link_libraries(test_wait_for_message ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_logger_service test_logger_service.cpp)
+if(TARGET test_logger_service)
+  ament_target_dependencies(test_logger_service
+    "rcl_interfaces")
+  target_link_libraries(test_logger_service ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_interface_traits test_interface_traits.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_interface_traits)
@@ -688,6 +708,39 @@ if(TARGET test_static_executor_entities_collector)
   target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
 endif()
 
+ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+if(TARGET test_entities_collector)
+  ament_target_dependencies(test_entities_collector
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_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)
+if(TARGET test_events_executor)
+  ament_target_dependencies(test_events_executor
+    "test_msgs")
+  target_link_libraries(test_events_executor ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_events_queue executors/test_events_queue.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_events_queue)
+  ament_target_dependencies(test_events_queue
+    "test_msgs")
+  target_link_libraries(test_events_queue ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_guard_condition test_guard_condition.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_guard_condition)

rclcpp/test/rclcpp/executors/test_entities_collector.cpp

@@ -0,0 +1,320 @@
+// 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

@@ -0,0 +1,549 @@
+// 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 <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+using namespace std::chrono_literals;
+
+using rclcpp::experimental::executors::EventsExecutor;
+
+class TestEventsExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+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
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  bool request_received = false;
+  bool response_received = false;
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [&request_received](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr)
+    {
+      request_received = true;
+    });
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  bool spin_exited = false;
+  std::thread spinner([&spin_exited, &executor, this]() {
+      executor.spin();
+      spin_exited = true;
+    });
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  client->async_send_request(
+    request,
+    [&response_received](rclcpp::Client<test_msgs::srv::Empty>::SharedFuture result_future) {
+      (void)result_future;
+      response_received = true;
+    });
+
+  // Wait some time for the client-server to be invoked
+  auto start = std::chrono::steady_clock::now();
+  while (
+    !response_received &&
+    !spin_exited &&
+    (std::chrono::steady_clock::now() - start < 1s))
+  {
+    std::this_thread::sleep_for(5ms);
+  }
+
+  executor.cancel();
+  spinner.join();
+  executor.remove_node(node);
+
+  EXPECT_TRUE(request_received);
+  EXPECT_TRUE(response_received);
+  EXPECT_TRUE(spin_exited);
+}
+
+TEST_F(TestEventsExecutor, spin_once_max_duration_timeout)
+{
+  // 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");
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  // Consume previous events so we have a fresh start
+  executor.spin_all(1s);
+
+  size_t t_runs = 0;
+  auto t = node->create_wall_timer(
+    10s,
+    [&]() {
+      t_runs++;
+    });
+
+  // This first spin_once takes care of the waitable event
+  // generated by the addition of the timer to the node
+  executor.spin_once(1s);
+  EXPECT_EQ(0u, t_runs);
+
+  auto start = std::chrono::steady_clock::now();
+
+  // This second spin_once should take care of the timer,
+  executor.spin_once(10ms);
+
+  // but doesn't spin the time enough to call the timer callback.
+  EXPECT_EQ(0u, t_runs);
+  EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+}
+
+TEST_F(TestEventsExecutor, spin_once_max_duration_timer)
+{
+  // 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");
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  // Consume previous events so we have a fresh start
+  executor.spin_all(1s);
+
+  size_t t_runs = 0;
+  auto t = node->create_wall_timer(
+    10ms,
+    [&]() {
+      t_runs++;
+    });
+
+  // This first spin_once takes care of the waitable event
+  // generated by the addition of the timer to the node
+  executor.spin_once(1s);
+  EXPECT_EQ(0u, t_runs);
+
+  auto start = std::chrono::steady_clock::now();
+
+  // This second spin_once should take care of the timer
+  executor.spin_once(11ms);
+
+  EXPECT_EQ(1u, t_runs);
+  EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+}
+
+TEST_F(TestEventsExecutor, spin_some_max_duration)
+{
+  // 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");
+
+    size_t t_runs = 0;
+    auto t = node->create_wall_timer(
+      10s,
+      [&]() {
+        t_runs++;
+      });
+
+    EventsExecutor executor;
+    executor.add_node(node);
+
+    auto start = std::chrono::steady_clock::now();
+    executor.spin_some(10ms);
+
+    EXPECT_EQ(0u, t_runs);
+    EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+  }
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    size_t t_runs = 0;
+    auto t = node->create_wall_timer(
+      10ms,
+      [&]() {
+        t_runs++;
+      });
+
+    // Sleep some time for the timer to be ready when spin
+    std::this_thread::sleep_for(10ms);
+
+    EventsExecutor executor;
+    executor.add_node(node);
+
+    auto start = std::chrono::steady_clock::now();
+    executor.spin_some(10s);
+
+    EXPECT_EQ(1u, t_runs);
+    EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+  }
+}
+
+TEST_F(TestEventsExecutor, spin_some_zero_duration)
+{
+  // 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");
+
+  size_t t_runs = 0;
+  auto t = node->create_wall_timer(
+    20ms,
+    [&]() {
+      t_runs++;
+    });
+
+  // Sleep some time for the timer to be ready when spin
+  std::this_thread::sleep_for(20ms);
+
+  EventsExecutor executor;
+  executor.add_node(node);
+  executor.spin_some(0ms);
+
+  EXPECT_EQ(1u, t_runs);
+}
+
+TEST_F(TestEventsExecutor, spin_all_max_duration)
+{
+  // 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");
+
+    size_t t_runs = 0;
+    auto t = node->create_wall_timer(
+      10s,
+      [&]() {
+        t_runs++;
+      });
+
+    EventsExecutor executor;
+    executor.add_node(node);
+
+    auto start = std::chrono::steady_clock::now();
+    executor.spin_all(10ms);
+
+    EXPECT_EQ(0u, t_runs);
+    EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+  }
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    size_t t_runs = 0;
+    auto t = node->create_wall_timer(
+      10ms,
+      [&]() {
+        t_runs++;
+      });
+
+    // Sleep some time for the timer to be ready when spin
+    std::this_thread::sleep_for(10ms);
+
+    EventsExecutor executor;
+    executor.add_node(node);
+
+    auto start = std::chrono::steady_clock::now();
+    executor.spin_all(10s);
+
+    EXPECT_EQ(1u, t_runs);
+    EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms);
+  }
+
+  EventsExecutor executor;
+  EXPECT_THROW(executor.spin_all(0ms), std::invalid_argument);
+  EXPECT_THROW(executor.spin_all(-5ms), std::invalid_argument);
+}
+
+TEST_F(TestEventsExecutor, cancel_while_timers_running)
+{
+  // 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");
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  // Take care of previous events for a fresh start
+  executor.spin_all(1s);
+
+  size_t t1_runs = 0;
+  auto t1 = node->create_wall_timer(
+    1ms,
+    [&]() {
+      t1_runs++;
+      std::this_thread::sleep_for(50ms);
+    });
+
+  size_t t2_runs = 0;
+  auto t2 = node->create_wall_timer(
+    1ms,
+    [&]() {
+      t2_runs++;
+      std::this_thread::sleep_for(50ms);
+    });
+
+
+  std::thread spinner([&executor, this]() {executor.spin();});
+
+  std::this_thread::sleep_for(10ms);
+  // Call cancel while t1 callback is still being executed
+  executor.cancel();
+  spinner.join();
+
+  // Depending on the latency on the system, t2 may start to execute before cancel is signaled
+  EXPECT_GE(1u, t1_runs);
+  EXPECT_GE(1u, t2_runs);
+}
+
+TEST_F(TestEventsExecutor, cancel_while_timers_waiting)
+{
+  // 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");
+
+  size_t t1_runs = 0;
+  auto t1 = node->create_wall_timer(
+    100s,
+    [&]() {
+      t1_runs++;
+    });
+
+  EventsExecutor executor;
+  executor.add_node(node);
+
+  auto start = std::chrono::steady_clock::now();
+  std::thread spinner([&executor, this]() {executor.spin();});
+
+  std::this_thread::sleep_for(10ms);
+  executor.cancel();
+  spinner.join();
+
+  EXPECT_EQ(0u, t1_runs);
+  EXPECT_TRUE(std::chrono::steady_clock::now() - start < 1s);
+}
+
+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();
+  }
+
+  // Create a publisher node and start publishing messages
+  auto node_pub = std::make_shared<rclcpp::Node>("node_pub");
+  auto publisher = node_pub->create_publisher<test_msgs::msg::Empty>("topic", rclcpp::QoS(10));
+  auto timer = node_pub->create_wall_timer(
+    2ms, [&]() {publisher->publish(std::make_unique<test_msgs::msg::Empty>());});
+  EventsExecutor executor_pub;
+  executor_pub.add_node(node_pub);
+  std::thread spinner([&executor_pub, this]() {executor_pub.spin();});
+
+  // Create a node with two different subscriptions to the topic
+  auto node_sub = std::make_shared<rclcpp::Node>("node_sub");
+  size_t callback_count_1 = 0;
+  auto subscription_1 =
+    node_sub->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_1++;});
+  size_t callback_count_2 = 0;
+  auto subscription_2 =
+    node_sub->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_2++;});
+  EventsExecutor executor_sub;
+  executor_sub.add_node(node_sub);
+
+  // Wait some time while messages are published
+  std::this_thread::sleep_for(10ms);
+
+  // Destroy one of the two subscriptions
+  subscription_1.reset();
+
+  // Let subscriptions executor spin
+  executor_sub.spin_some(10ms);
+
+  // The callback count of the destroyed subscription remained at 0
+  EXPECT_EQ(0u, callback_count_1);
+  EXPECT_LT(0u, callback_count_2);
+
+  executor_pub.cancel();
+  spinner.join();
+}
+
+// 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;
+TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks)
+{
+  // 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");
+  rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler();
+
+  std::string pub_log_msg;
+  std::string sub_log_msg;
+  std::promise<void> log_msgs_promise;
+  g_pub_log_msg = &pub_log_msg;
+  g_sub_log_msg = &sub_log_msg;
+  g_log_msgs_promise = &log_msgs_promise;
+  auto logger_callback = [](
+    const rcutils_log_location_t * /*location*/,
+    int /*level*/, const char * /*name*/, rcutils_time_point_value_t /*timestamp*/,
+    const char * format, va_list * args) -> void {
+      char buffer[1024];
+      vsnprintf(buffer, sizeof(buffer), format, *args);
+      const std::string msg = buffer;
+      if (msg.rfind("New subscription discovered on topic '/test_topic'", 0) == 0) {
+        *g_pub_log_msg = buffer;
+      } else if (msg.rfind("New publisher discovered on topic '/test_topic'", 0) == 0) {
+        *g_sub_log_msg = buffer;
+      }
+
+      if (!g_pub_log_msg->empty() && !g_sub_log_msg->empty()) {
+        g_log_msgs_promise->set_value();
+      }
+    };
+  rcutils_logging_set_output_handler(logger_callback);
+
+  std::shared_future<void> log_msgs_future = log_msgs_promise.get_future();
+
+  rclcpp::QoS qos_profile_publisher(10);
+  qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>(
+    "test_topic", qos_profile_publisher);
+
+  rclcpp::QoS qos_profile_subscription(10);
+  qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    "test_topic", qos_profile_subscription, [&](test_msgs::msg::Empty::ConstSharedPtr) {});
+
+  EventsExecutor ex;
+  ex.add_node(node->get_node_base_interface());
+
+  const auto timeout = std::chrono::seconds(10);
+  ex.spin_until_future_complete(log_msgs_future, timeout);
+
+  EXPECT_EQ(
+    "New subscription discovered on topic '/test_topic', requesting incompatible QoS. "
+    "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
+    pub_log_msg);
+  EXPECT_EQ(
+    "New publisher discovered on topic '/test_topic', offering incompatible QoS. "
+    "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
+    sub_log_msg);
+
+  rcutils_logging_set_output_handler(original_output_handler);
+}

rclcpp/test/rclcpp/executors/test_events_queue.cpp

@@ -0,0 +1,82 @@
+// 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 <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestEventsQueue, SimpleQueueTest)
+{
+  // Create a SimpleEventsQueue and a local queue
+  auto simple_queue = std::make_unique<rclcpp::experimental::executors::SimpleEventsQueue>();
+  rclcpp::experimental::executors::ExecutorEvent event {};
+  bool ret = false;
+
+  // Make sure the queue is empty at startup
+  EXPECT_TRUE(simple_queue->empty());
+  EXPECT_EQ(simple_queue->size(), 0u);
+
+  // Push 11 messages
+  for (uint32_t i = 1; i < 11; i++) {
+    rclcpp::experimental::executors::ExecutorEvent stub_event {};
+    stub_event.num_events = 1;
+    simple_queue->enqueue(stub_event);
+
+    EXPECT_FALSE(simple_queue->empty());
+    EXPECT_EQ(simple_queue->size(), i);
+  }
+
+  // Pop one message
+  ret = simple_queue->dequeue(event);
+  EXPECT_TRUE(ret);
+  EXPECT_FALSE(simple_queue->empty());
+  EXPECT_EQ(simple_queue->size(), 9u);
+
+  // Pop one message
+  ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0));
+  EXPECT_TRUE(ret);
+  EXPECT_FALSE(simple_queue->empty());
+  EXPECT_EQ(simple_queue->size(), 8u);
+
+  while (!simple_queue->empty()) {
+    ret = simple_queue->dequeue(event);
+    EXPECT_TRUE(ret);
+  }
+
+  EXPECT_TRUE(simple_queue->empty());
+  EXPECT_EQ(simple_queue->size(), 0u);
+
+  ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0));
+  EXPECT_FALSE(ret);
+
+  // Lets push an event into the queue and get it back
+  rclcpp::experimental::executors::ExecutorEvent push_event = {
+    simple_queue.get(),
+    99,
+    rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT,
+    1};
+
+  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.type, event.type);
+  EXPECT_EQ(push_event.num_events, event.num_events);
+}

rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp

@@ -0,0 +1,97 @@
+// 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

@@ -20,12 +20,14 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <atomic>
 #include <chrono>
 #include <limits>
 #include <memory>
 #include <string>
 #include <thread>
 #include <utility>
+#include <vector>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
@@ -43,18 +45,10 @@ template<typename T>
 class TestExecutors : public ::testing::Test
 {
 public:
-  static void SetUpTestCase()
-  {
-    rclcpp::init(0, nullptr);
-  }
-
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
-  }
-
   void SetUp()
   {
+    rclcpp::init(0, nullptr);
+
     const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
     std::stringstream test_name;
     test_name << test_info->test_case_name() << "_" << test_info->name();
@@ -75,6 +69,8 @@ public:
     publisher.reset();
     subscription.reset();
     node.reset();
+
+    rclcpp::shutdown();
   }
 
   rclcpp::Node::SharedPtr node;
@@ -92,7 +88,8 @@ using ExecutorTypes =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
   rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::executors::StaticSingleThreadedExecutor>;
+  rclcpp::executors::StaticSingleThreadedExecutor,
+  rclcpp::experimental::executors::EventsExecutor>;
 
 class ExecutorTypeNames
 {
@@ -113,6 +110,10 @@ public:
       return "StaticSingleThreadedExecutor";
     }
 
+    if (std::is_same<T, rclcpp::experimental::executors::EventsExecutor>()) {
+      return "EventsExecutor";
+    }
+
     return "";
   }
 };
@@ -126,12 +127,22 @@ TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
 using StandardExecutors =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
-  rclcpp::executors::MultiThreadedExecutor>;
+  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) {
+TYPED_TEST(TestExecutors, detachOnDestruction)
+{
   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;
     executor.add_node(this->node);
@@ -145,8 +156,17 @@ TYPED_TEST(TestExecutors, detachOnDestruction) {
 // Make sure that the executor can automatically remove expired nodes correctly
 // Currently fails for StaticSingleThreadedExecutor so it is being skipped, see:
 // https://github.com/ros2/rclcpp/issues/1231
-TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
+TYPED_TEST(TestExecutorsStable, addTemporaryNode)
+{
   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;
 
   {
@@ -163,9 +183,37 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
   spinner.join();
 }
 
-// Check executor throws properly if the same node is added a second time
-TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
+// 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)
+{
+  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 executor1;
   ExecutorType executor2;
   EXPECT_NO_THROW(executor1.add_node(this->node));
@@ -174,8 +222,17 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
 }
 
 // Check simple spin example
-TYPED_TEST(TestExecutors, spinWithTimer) {
+TYPED_TEST(TestExecutors, spinWithTimer)
+{
   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;
 
   bool timer_completed = false;
@@ -196,8 +253,17 @@ TYPED_TEST(TestExecutors, spinWithTimer) {
   executor.remove_node(this->node, true);
 }
 
-TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
+TYPED_TEST(TestExecutors, spinWhileAlreadySpinning)
+{
   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;
   executor.add_node(this->node);
 
@@ -222,8 +288,17 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
 }
 
 // Check executor exits immediately if future is complete.
-TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
+TYPED_TEST(TestExecutors, testSpinUntilFutureComplete)
+{
   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;
   executor.add_node(this->node);
 
@@ -244,8 +319,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
 }
 
 // Same test, but uses a shared future.
-TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) {
+TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete)
+{
   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;
   executor.add_node(this->node);
 
@@ -267,8 +351,17 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) {
 }
 
 // For a longer running future that should require several iterations of spin_once
-TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout)
+{
   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;
   executor.add_node(this->node);
 
@@ -313,8 +406,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
 }
 
 // Check spin_until_future_complete timeout works as expected
-TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) {
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout)
+{
   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;
   executor.add_node(this->node);
 
@@ -380,6 +482,13 @@ public:
     return nullptr;
   }
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void) id;
+    return nullptr;
+  }
+
   void
   execute(std::shared_ptr<void> & data) override
   {
@@ -388,6 +497,21 @@ public:
     std::this_thread::sleep_for(3ms);
   }
 
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    auto gc_callback = [callback](size_t count) {
+        callback(count, 0);
+      };
+    gc_.set_on_trigger_callback(gc_callback);
+  }
+
+  void
+  clear_on_ready_callback() override
+  {
+    gc_.set_on_trigger_callback(nullptr);
+  }
+
   size_t
   get_number_of_ready_guard_conditions() override {return 1;}
 
@@ -402,8 +526,17 @@ private:
   rclcpp::GuardCondition gc_;
 };
 
-TYPED_TEST(TestExecutors, spinAll) {
+TYPED_TEST(TestExecutors, spinAll)
+{
   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;
   auto waitable_interfaces = this->node->get_node_waitables_interface();
   auto my_waitable = std::make_shared<TestWaitable>();
@@ -443,8 +576,17 @@ TYPED_TEST(TestExecutors, spinAll) {
   spinner.join();
 }
 
-TYPED_TEST(TestExecutors, spinSome) {
+TYPED_TEST(TestExecutors, spinSome)
+{
   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;
   auto waitable_interfaces = this->node->get_node_waitables_interface();
   auto my_waitable = std::make_shared<TestWaitable>();
@@ -472,8 +614,9 @@ TYPED_TEST(TestExecutors, spinSome) {
     this->publisher->publish(test_msgs::msg::Empty());
     std::this_thread::sleep_for(1ms);
   }
-
-  EXPECT_EQ(1u, my_waitable->get_count());
+  // 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());
   waitable_interfaces->remove_waitable(my_waitable, nullptr);
   EXPECT_TRUE(spin_exited);
   // Cancel if it hasn't exited already.
@@ -483,8 +626,17 @@ TYPED_TEST(TestExecutors, spinSome) {
 }
 
 // Check spin_node_until_future_complete with node base pointer
-TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) {
+TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr)
+{
   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::promise<bool> promise;
@@ -498,8 +650,17 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) {
 }
 
 // Check spin_node_until_future_complete with node pointer
-TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) {
+TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr)
+{
   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::promise<bool> promise;
@@ -513,8 +674,17 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) {
 }
 
 // Check spin_until_future_complete can be properly interrupted.
-TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) {
+TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted)
+{
   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;
   executor.add_node(this->node);
 
@@ -555,8 +725,80 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) {
   spinner.join();
 }
 
+// This test verifies that the add_node operation is robust wrt race conditions.
+// It's mostly meant to prevent regressions in the events-executor, but the operation should be
+// thread-safe in all executor implementations.
+// The initial implementation of the events-executor contained a bug where the executor
+// would end up in an inconsistent state and stop processing interrupt/shutdown notifications.
+// Manually adding a node to the executor results in a) producing a notify waitable event
+// and b) refreshing the executor collections.
+// The inconsistent state would happen if the event was processed before the collections were
+// finished to be refreshed: the executor would pick up the event but be unable to process it.
+// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional
+// notify waitable events to be pushed.
+// The behavior is observable only under heavy load, so this test spawns several worker
+// threads. Due to the nature of the bug, this test may still succeed even if the
+// bug is present. However repeated runs will show its flakiness nature and indicate
+// an eventual regression.
+TYPED_TEST(TestExecutors, testRaceConditionAddNode)
+{
+  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();
+  }
+
+  // Spawn some threads to do some heavy work
+  std::atomic<bool> should_cancel = false;
+  std::vector<std::thread> stress_threads;
+  for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) {
+    stress_threads.emplace_back(
+      [&should_cancel, i]() {
+        // This is just some arbitrary heavy work
+        volatile size_t total = 0;
+        for (size_t k = 0; k < 549528914167; k++) {
+          if (should_cancel) {
+            break;
+          }
+          total += k * (i + 42);
+        }
+      });
+  }
+
+  // Create an executor
+  auto executor = std::make_shared<ExecutorType>();
+  // Start spinning
+  auto executor_thread = std::thread(
+    [executor]() {
+      executor->spin();
+    });
+  // Add a node to the executor
+  executor->add_node(this->node);
+
+  // Cancel the executor (make sure that it's already spinning first)
+  while (!executor->is_spinning() && rclcpp::ok()) {
+    continue;
+  }
+  executor->cancel();
+
+  // Try to join the thread after cancelling the executor
+  // This is the "test". We want to make sure that we can still cancel the executor
+  // regardless of the presence of race conditions
+  executor_thread.join();
+
+  // The test is now completed: we can join the stress threads
+  should_cancel = true;
+  for (auto & t : stress_threads) {
+    t.join();
+  }
+}
+
 // Check spin_until_future_complete with node base pointer (instantiates its own executor)
-TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) {
+TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr)
+{
   rclcpp::init(0, nullptr);
 
   {
@@ -576,7 +818,8 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) {
 }
 
 // Check spin_until_future_complete with node pointer (instantiates its own executor)
-TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) {
+TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr)
+{
   rclcpp::init(0, nullptr);
 
   {
@@ -593,3 +836,106 @@ 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 = 0u;
+
+    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(1u);
+      };
+
+    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_size_t 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(0u, 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(0u);
+
+  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,6 +28,7 @@
 #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"
 
@@ -599,6 +600,18 @@ 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_parameters.cpp

@@ -77,9 +77,9 @@ TEST_F(TestNodeParameters, list_parameters)
   std::vector<std::string> prefixes;
   const auto list_result = node_parameters->list_parameters(prefixes, 1u);
 
-  // Currently the only default parameter is 'use_sim_time', but that may change.
+  // Currently the default parameters are 'use_sim_time' and 'start_type_description_service'
   size_t number_of_parameters = list_result.names.size();
-  EXPECT_GE(1u, number_of_parameters);
+  EXPECT_GE(2u, number_of_parameters);
 
   const std::string parameter_name = "new_parameter";
   const rclcpp::ParameterValue value(true);

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -28,6 +28,11 @@
 #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
 {
 
@@ -77,6 +82,18 @@ 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/node_interfaces/test_node_type_descriptions.cpp

@@ -0,0 +1,63 @@
+// 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/node.hpp"
+#include "rclcpp/node_interfaces/node_type_descriptions.hpp"
+
+class TestNodeTypeDescriptions : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestNodeTypeDescriptions, interface_created)
+{
+  rclcpp::Node node{"node", "ns"};
+  ASSERT_NE(nullptr, node.get_node_type_descriptions_interface());
+}
+
+TEST_F(TestNodeTypeDescriptions, disabled_no_service)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.append_parameter_override("start_type_description_service", false);
+  rclcpp::Node node{"node", "ns", node_options};
+
+  rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle();
+  rcl_service_t * srv = nullptr;
+  rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv);
+  ASSERT_EQ(RCL_RET_NOT_INIT, ret);
+}
+
+TEST_F(TestNodeTypeDescriptions, enabled_creates_service)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.append_parameter_override("start_type_description_service", true);
+  rclcpp::Node node{"node", "ns", node_options};
+
+  rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle();
+  rcl_service_t * srv = nullptr;
+  rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  ASSERT_NE(nullptr, srv);
+}

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -46,11 +46,15 @@ public:
   }
 };
 
+template<typename T>
+class TestAddCallbackGroupsToExecutorStable : public TestAddCallbackGroupsToExecutor<T> {};
+
 using ExecutorTypes =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
   rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::executors::StaticSingleThreadedExecutor>;
+  rclcpp::executors::StaticSingleThreadedExecutor,
+  rclcpp::experimental::executors::EventsExecutor>;
 
 class ExecutorTypeNames
 {
@@ -71,17 +75,38 @@ public:
       return "StaticSingleThreadedExecutor";
     }
 
+    if (std::is_same<T, rclcpp::experimental::executors::EventsExecutor>()) {
+      return "EventsExecutor";
+    }
+
     return "";
   }
 };
 
 TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames);
 
+// StaticSingleThreadedExecutor is not included in these tests for now
+using StandardExecutors =
+  ::testing::Types<
+  rclcpp::executors::SingleThreadedExecutor,
+  rclcpp::executors::MultiThreadedExecutor,
+  rclcpp::experimental::executors::EventsExecutor>;
+TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, ExecutorTypeNames);
+
 /*
  * Test adding callback groups.
  */
-TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) {
+TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups)
+{
   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();
+  }
+
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
@@ -127,8 +152,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) {
 /*
  * Test removing callback groups.
  */
-TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) {
+TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups)
+{
   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();
+  }
+
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
@@ -158,7 +192,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) {
  */
 TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups)
 {
-  rclcpp::executors::MultiThreadedExecutor executor;
+  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;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
@@ -176,7 +219,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups)
  */
 TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor)
 {
-  rclcpp::executors::MultiThreadedExecutor executor;
+  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;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   executor.add_node(node->get_node_base_interface());
   ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
@@ -210,13 +262,22 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t
  */
 TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups)
 {
+  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;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive, false);
   rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
     2s, timer_callback, cb_grp);
-  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_callback_group(cb_grp, node->get_node_base_interface());
   ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
 
@@ -245,14 +306,23 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups)
  */
 TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors)
 {
+  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 timer_executor;
+  ExecutorType sub_executor;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive, false);
   rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer(
     2s, timer_callback, cb_grp);
-  rclcpp::executors::MultiThreadedExecutor timer_executor;
-  rclcpp::executors::MultiThreadedExecutor sub_executor;
   timer_executor.add_callback_group(cb_grp, node->get_node_base_interface());
   const rclcpp::QoS qos(10);
   auto options = rclcpp::SubscriptionOptions();
@@ -282,14 +352,23 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e
  * because the executor can't be triggered while a subscriber created, see
  * https://github.com/ros2/rclcpp/issues/1611
 */
-TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message)
+TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message)
 {
+  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();
+  }
+
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
 
   // create a thread running an executor with a new callback group for a coming subscriber
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive, false);
-  rclcpp::executors::SingleThreadedExecutor cb_grp_executor;
+  ExecutorType cb_grp_executor;
 
   std::promise<bool> received_message_promise;
   auto received_message_future = received_message_promise.get_future();
@@ -329,7 +408,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess
       timer_promise.set_value();
     };
 
-  rclcpp::executors::SingleThreadedExecutor timer_executor;
+  ExecutorType timer_executor;
   timer = node->create_wall_timer(100ms, timer_callback);
   timer_executor.add_node(node);
   auto future = timer_promise.get_future();
@@ -346,8 +425,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess
  * because the executor can't be triggered while a subscriber created, see
  * https://github.com/ros2/rclcpp/issues/2067
 */
-TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin)
+TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin)
 {
+  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();
+  }
+
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
 
   // create a publisher to send data
@@ -357,7 +445,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin)
   publisher->publish(test_msgs::msg::Empty());
 
   // create a thread running an executor
-  rclcpp::executors::SingleThreadedExecutor executor;
+  ExecutorType executor;
   executor.add_node(node);
   std::promise<bool> received_message_promise;
   auto received_message_future = received_message_promise.get_future();
@@ -392,7 +480,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin)
  */
 TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group)
 {
-  rclcpp::executors::MultiThreadedExecutor executor;
+  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;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto timer_callback = []() {};
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(

rclcpp/test/rclcpp/test_logger_service.cpp

@@ -0,0 +1,214 @@
+// Copyright 2023 Sony Group 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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/node.hpp"
+#include "rcl_interfaces/srv/get_logger_levels.hpp"
+#include "rcl_interfaces/srv/set_logger_levels.hpp"
+
+using namespace std::chrono_literals;
+
+class TestLoggerService : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options = rclcpp::NodeOptions();
+    options.enable_logger_service(true);
+    node_ = std::make_shared<rclcpp::Node>("test_logger_service", "/test", options);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+  rclcpp::Node::SharedPtr node_;
+  std::thread thread_;
+};
+
+TEST_F(TestLoggerService, check_connect_get_logger_service) {
+  auto client = node_->create_client<rcl_interfaces::srv::GetLoggerLevels>(
+    "/test/test_logger_service/get_logger_levels");
+  ASSERT_TRUE(client->wait_for_service(2s));
+}
+
+TEST_F(TestLoggerService, check_connect_set_logger_service) {
+  auto client = node_->create_client<rcl_interfaces::srv::SetLoggerLevels>(
+    "/test/test_logger_service/set_logger_levels");
+  ASSERT_TRUE(client->wait_for_service(2s));
+}
+
+TEST_F(TestLoggerService, test_set_and_get_one_logging_level) {
+  std::string test_logger_name = "rcl";
+  uint8_t test_logger_level = 20;
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::SetLoggerLevels>(
+      "/test/test_logger_service/set_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::SetLoggerLevels::Request>();
+    auto level = rcl_interfaces::msg::LoggerLevel();
+    level.name = test_logger_name;
+    level.level = test_logger_level;
+    request->levels.push_back(level);
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->results.size(), 1u);
+    ASSERT_TRUE(result_get->results[0].successful);
+    ASSERT_STREQ(result_get->results[0].reason.c_str(), "");
+  }
+
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::GetLoggerLevels>(
+      "/test/test_logger_service/get_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::GetLoggerLevels::Request>();
+    request->names.emplace_back(test_logger_name);
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->levels.size(), 1u);
+    ASSERT_STREQ(result_get->levels[0].name.c_str(), test_logger_name.c_str());
+    ASSERT_EQ(result_get->levels[0].level, test_logger_level);
+  }
+}
+
+TEST_F(TestLoggerService, test_set_and_get_multi_logging_level) {
+  std::vector<std::pair<std::string, uint8_t>> test_data {
+    {"rcl", 30},
+    {"rclcpp", 40},
+    {"/test/test_logger_service", 50}
+  };
+
+  // Set multi log levels
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::SetLoggerLevels>(
+      "/test/test_logger_service/set_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::SetLoggerLevels::Request>();
+    for (auto & set_level : test_data) {
+      auto level = rcl_interfaces::msg::LoggerLevel();
+      level.name = std::get<0>(set_level);
+      level.level = std::get<1>(set_level);
+      request->levels.push_back(level);
+    }
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->results.size(), test_data.size());
+    for (uint32_t i = 0; i < test_data.size(); i++) {
+      ASSERT_TRUE(result_get->results[0].successful);
+    }
+  }
+
+  // Get multi log levels
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::GetLoggerLevels>(
+      "/test/test_logger_service/get_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::GetLoggerLevels::Request>();
+    for (auto & set_level : test_data) {
+      request->names.emplace_back(std::get<0>(set_level));
+    }
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->levels.size(), test_data.size());
+    for (uint32_t i = 0; i < test_data.size(); i++) {
+      ASSERT_STREQ(result_get->levels[i].name.c_str(), std::get<0>(test_data[i]).c_str());
+      ASSERT_EQ(result_get->levels[i].level, std::get<1>(test_data[i]));
+    }
+  }
+}
+
+TEST_F(TestLoggerService, test_set_logging_level_with_invalid_param) {
+  std::vector<std::pair<std::string, uint8_t>> test_data {
+    {"rcl", 12},
+    {"/test/test_logger_service", 22}
+  };
+
+  // Set multi log levels
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::SetLoggerLevels>(
+      "/test/test_logger_service/set_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::SetLoggerLevels::Request>();
+    for (auto & set_level : test_data) {
+      auto level = rcl_interfaces::msg::LoggerLevel();
+      level.name = std::get<0>(set_level);
+      level.level = std::get<1>(set_level);
+      request->levels.push_back(level);
+    }
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->results.size(), test_data.size());
+    for (uint32_t i = 0; i < test_data.size(); i++) {
+      ASSERT_FALSE(result_get->results[i].successful);
+      // Check string starts with prefix
+      ASSERT_EQ(
+        result_get->results[i].reason.rfind("Unable to determine severity_string for severity", 0),
+        0);
+    }
+  }
+}
+
+TEST_F(TestLoggerService, test_set_logging_level_with_partial_invalid_param) {
+  std::vector<std::pair<std::string, uint8_t>> test_data {
+    {"rcl", 20},
+    {"rclcpp", 22},
+    {"/test/test_logger_service", 30}
+  };
+
+  // Set multi log levels
+  {
+    auto client = node_->create_client<rcl_interfaces::srv::SetLoggerLevels>(
+      "/test/test_logger_service/set_logger_levels");
+    ASSERT_TRUE(client->wait_for_service(1s));
+    auto request = std::make_shared<rcl_interfaces::srv::SetLoggerLevels::Request>();
+    for (auto & set_level : test_data) {
+      auto level = rcl_interfaces::msg::LoggerLevel();
+      level.name = std::get<0>(set_level);
+      level.level = std::get<1>(set_level);
+      request->levels.push_back(level);
+    }
+    auto result = client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::spin_until_future_complete(node_, result),
+      rclcpp::FutureReturnCode::SUCCESS);
+    auto result_get = result.get();
+    ASSERT_EQ(result_get->results.size(), test_data.size());
+    ASSERT_TRUE(result_get->results[0].successful);
+    ASSERT_FALSE(result_get->results[1].successful);
+    ASSERT_TRUE(result_get->results[2].successful);
+  }
+}

rclcpp/test/rclcpp/test_node.cpp

@@ -78,6 +78,7 @@ TEST_F(TestNode, construction_and_destruction) {
     EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options());
     EXPECT_NE(nullptr, node->get_graph_event());
     EXPECT_NE(nullptr, node->get_clock());
+    EXPECT_NE(nullptr, node->get_node_type_descriptions_interface());
   }
 
   {

rclcpp/test/rclcpp/test_node_options.cpp

@@ -266,6 +266,11 @@ TEST(TestNodeOptions, bool_setters_and_getters) {
   EXPECT_FALSE(options.automatically_declare_parameters_from_overrides());
   options.automatically_declare_parameters_from_overrides(true);
   EXPECT_TRUE(options.automatically_declare_parameters_from_overrides());
+
+  options.enable_logger_service(false);
+  EXPECT_FALSE(options.enable_logger_service());
+  options.enable_logger_service(true);
+  EXPECT_TRUE(options.enable_logger_service());
 }
 
 TEST(TestNodeOptions, parameter_event_qos) {

rclcpp/test/rclcpp/test_parameter_client.cpp

@@ -59,6 +59,8 @@ protected:
     node_with_option.reset();
   }
 
+  // "start_type_description_service" and "use_sim_time"
+  const uint64_t builtin_param_count = 2;
   rclcpp::Node::SharedPtr node;
   rclcpp::Node::SharedPtr node_with_option;
 };
@@ -925,6 +927,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) {
   Coverage for async load_parameters
  */
 TEST_F(TestParameterClient, async_parameter_load_parameters) {
+  const uint64_t expected_param_count = 4 + builtin_param_count;
   auto load_node = std::make_shared<rclcpp::Node>(
     "load_node",
     "namespace",
@@ -944,12 +947,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) {
   auto list_parameters = asynchronous_client->list_parameters({}, 3);
   rclcpp::spin_until_future_complete(
     load_node, list_parameters, std::chrono::milliseconds(100));
-  ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(5));
+  ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
 }
 /*
   Coverage for sync load_parameters
  */
 TEST_F(TestParameterClient, sync_parameter_load_parameters) {
+  const uint64_t expected_param_count = 4 + builtin_param_count;
   auto load_node = std::make_shared<rclcpp::Node>(
     "load_node",
     "namespace",
@@ -964,13 +968,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) {
   ASSERT_EQ(load_future[0].successful, true);
   // list parameters
   auto list_parameters = synchronous_client->list_parameters({}, 3);
-  ASSERT_EQ(list_parameters.names.size(), static_cast<uint64_t>(5));
+  ASSERT_EQ(list_parameters.names.size(), static_cast<uint64_t>(expected_param_count));
 }
 
 /*
   Coverage for async load_parameters with complicated regex expression
  */
 TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) {
+  const uint64_t expected_param_count = 5 + builtin_param_count;
   auto load_node = std::make_shared<rclcpp::Node>(
     "load_node",
     "namespace",
@@ -990,7 +995,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) {
   auto list_parameters = asynchronous_client->list_parameters({}, 3);
   rclcpp::spin_until_future_complete(
     load_node, list_parameters, std::chrono::milliseconds(100));
-  ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(6));
+  ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
   // to check the parameter "a_value"
   std::string param_name = "a_value";
   auto param = load_node->get_parameter(param_name);
@@ -1020,6 +1025,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) {
   Coverage for async load_parameters from maps with complicated regex expression
  */
 TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) {
+  const uint64_t expected_param_count = 5 + builtin_param_count;
   auto load_node = std::make_shared<rclcpp::Node>(
     "load_node",
     "namespace",
@@ -1068,7 +1074,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) {
   auto list_parameters = asynchronous_client->list_parameters({}, 3);
   rclcpp::spin_until_future_complete(
     load_node, list_parameters, std::chrono::milliseconds(100));
-  ASSERT_EQ(list_parameters.get().names.size(), static_cast<uint64_t>(6));
+  ASSERT_EQ(list_parameters.get().names.size(), expected_param_count);
   // to check the parameter "a_value"
   std::string param_name = "a_value";
   auto param = load_node->get_parameter(param_name);

rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp

@@ -152,33 +152,54 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) {
   }
 }
 
+using UseTakeSharedMethod = bool;
+class TestPublisherFixture
+  : public TestPublisher,
+  public ::testing::WithParamInterface<UseTakeSharedMethod>
+{
+};
+
 /*
  * Testing that publisher sends type adapted types and ROS message types with intra proccess communications.
  */
-TEST_F(
-  TestPublisher,
+TEST_P(
+  TestPublisherFixture,
   check_type_adapted_message_is_sent_and_received_intra_process) {
   using StringTypeAdapter = rclcpp::TypeAdapter<std::string, rclcpp::msg::String>;
   const std::string message_data = "Message Data";
   const std::string topic_name = "topic_name";
   bool is_received;
 
-  auto callback =
-    [message_data, &is_received](
-    const rclcpp::msg::String::ConstSharedPtr msg,
-    const rclcpp::MessageInfo & message_info
-    ) -> void
-    {
-      is_received = true;
-      ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
-      ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
-    };
-
   auto node = rclcpp::Node::make_shared(
     "test_intra_process",
     rclcpp::NodeOptions().use_intra_process_comms(true));
   auto pub = node->create_publisher<StringTypeAdapter>(topic_name, 10);
-  auto sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback);
+  rclcpp::Subscription<rclcpp::msg::String>::SharedPtr sub;
+  if (GetParam()) {
+    auto callback =
+      [message_data, &is_received](
+      const rclcpp::msg::String::ConstSharedPtr msg,
+      const rclcpp::MessageInfo & message_info
+      ) -> void
+      {
+        is_received = true;
+        ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
+        ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
+      };
+    sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback);
+  } else {
+    auto callback_unique =
+      [message_data, &is_received](
+      rclcpp::msg::String::UniquePtr msg,
+      const rclcpp::MessageInfo & message_info
+      ) -> void
+      {
+        is_received = true;
+        ASSERT_STREQ(message_data.c_str(), msg->data.c_str());
+        ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process);
+      };
+    sub = node->create_subscription<rclcpp::msg::String>(topic_name, 1, callback_unique);
+  }
 
   auto wait_for_message_to_be_received = [&is_received, &node]() {
       rclcpp::executors::SingleThreadedExecutor executor;
@@ -239,6 +260,14 @@ TEST_F(
   }
 }
 
+INSTANTIATE_TEST_SUITE_P(
+  TestPublisherFixtureWithParam,
+  TestPublisherFixture,
+  ::testing::Values(
+    true,   // use take shared method
+    false   // not use take shared method
+));
+
 /*
  * Testing that publisher sends type adapted types and ROS message types with inter proccess communications.
  */

rclcpp/test/rclcpp/test_rate.cpp

@@ -18,10 +18,24 @@
 
 #include "rclcpp/rate.hpp"
 
+class TestRate : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
 /*
    Basic tests for the Rate and WallRate classes.
  */
-TEST(TestRate, rate_basics) {
+TEST_F(TestRate, rate_basics) {
   auto period = std::chrono::milliseconds(1000);
   auto offset = std::chrono::milliseconds(500);
   auto epsilon = std::chrono::milliseconds(100);
@@ -61,7 +75,7 @@ TEST(TestRate, rate_basics) {
   ASSERT_TRUE(epsilon > delta);
 }
 
-TEST(TestRate, wall_rate_basics) {
+TEST_F(TestRate, wall_rate_basics) {
   auto period = std::chrono::milliseconds(100);
   auto offset = std::chrono::milliseconds(50);
   auto epsilon = std::chrono::milliseconds(1);
@@ -101,7 +115,7 @@ TEST(TestRate, wall_rate_basics) {
   EXPECT_GT(epsilon, delta);
 }
 
-TEST(TestRate, from_double) {
+TEST_F(TestRate, from_double) {
   {
     rclcpp::WallRate rate(1.0);
     EXPECT_EQ(std::chrono::seconds(1), rate.period());

rclcpp/test/rclcpp/test_subscription_content_filter.cpp

@@ -310,3 +310,25 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil
     }
   }
 }
+
+TEST_F(
+  CLASSNAME(
+    TestContentFilterSubscription,
+    RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) {
+
+  // Create another content filter
+  auto options = rclcpp::SubscriptionOptions();
+
+  std::string filter_expression = "int32_value > %0";
+  std::vector<std::string> expression_parameters = {"4"};
+
+  options.content_filter_options.filter_expression = filter_expression;
+  options.content_filter_options.expression_parameters = expression_parameters;
+
+  auto callback = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub_2 = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "content_filter_topic", qos, callback, options);
+
+  EXPECT_NE(nullptr, sub_2);
+  sub_2.reset();
+}

rclcpp/test/rclcpp/test_timers_manager.cpp

@@ -0,0 +1,419 @@
+// 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 <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/experimental/timers_manager.hpp"
+
+using namespace std::chrono_literals;
+
+using rclcpp::experimental::TimersManager;
+
+using CallbackT = std::function<void ()>;
+using TimerT = rclcpp::WallTimer<CallbackT>;
+
+class TestTimersManager : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+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>(
+    rclcpp::contexts::get_global_default_context());
+
+  EXPECT_EQ(std::chrono::nanoseconds::max(), timers_manager->get_head_timeout());
+  EXPECT_FALSE(timers_manager->execute_head_timer());
+  EXPECT_NO_THROW(timers_manager->clear());
+  EXPECT_NO_THROW(timers_manager->start());
+  EXPECT_NO_THROW(timers_manager->stop());
+}
+
+TEST_F(TestTimersManager, add_run_remove_timer)
+{
+  size_t t_runs = 0;
+  auto t = TimerT::make_shared(
+    10ms,
+    [&t_runs]() {
+      t_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+  std::weak_ptr<TimerT> t_weak = t;
+
+  // Add the timer to the timers manager
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+  timers_manager->add_timer(t);
+
+  // Sleep for more 3 times the timer period
+  std::this_thread::sleep_for(30ms);
+
+  // The timer is executed only once, even if we slept 3 times the period
+  execute_all_ready_timers(timers_manager);
+  EXPECT_EQ(1u, t_runs);
+
+  // Remove the timer from the manager
+  timers_manager->remove_timer(t);
+
+  t.reset();
+  // The timer is now not valid anymore
+  EXPECT_FALSE(t_weak.lock() != nullptr);
+}
+
+TEST_F(TestTimersManager, clear)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  auto t1 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context());
+  std::weak_ptr<TimerT> t1_weak = t1;
+  auto t2 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context());
+  std::weak_ptr<TimerT> t2_weak = t2;
+
+  timers_manager->add_timer(t1);
+  timers_manager->add_timer(t2);
+
+  EXPECT_TRUE(t1_weak.lock() != nullptr);
+  EXPECT_TRUE(t2_weak.lock() != nullptr);
+
+  timers_manager->clear();
+
+  t1.reset();
+  t2.reset();
+
+  EXPECT_FALSE(t1_weak.lock() != nullptr);
+  EXPECT_FALSE(t2_weak.lock() != nullptr);
+}
+
+TEST_F(TestTimersManager, remove_not_existing_timer)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  // Try to remove a nullptr timer
+  EXPECT_NO_THROW(timers_manager->remove_timer(nullptr));
+
+  auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context());
+  timers_manager->add_timer(t);
+
+  // Remove twice the same timer
+  timers_manager->remove_timer(t);
+  EXPECT_NO_THROW(timers_manager->remove_timer(t));
+}
+
+TEST_F(TestTimersManager, timers_thread_exclusive_usage)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  timers_manager->start();
+
+  EXPECT_THROW(timers_manager->start(), std::exception);
+  EXPECT_THROW(timers_manager->get_head_timeout(), 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_head_timer());
+}
+
+TEST_F(TestTimersManager, add_timer_twice)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context());
+
+  timers_manager->add_timer(t);
+  EXPECT_NO_THROW(timers_manager->add_timer(t));
+}
+
+TEST_F(TestTimersManager, add_nullptr)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  EXPECT_THROW(timers_manager->add_timer(nullptr), std::exception);
+}
+
+TEST_F(TestTimersManager, head_not_ready)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t_runs = 0;
+  auto t = TimerT::make_shared(
+    10s,
+    [&t_runs]() {
+      t_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  timers_manager->add_timer(t);
+
+  // Timer will take 10s to get ready, so nothing to execute here
+  bool ret = timers_manager->execute_head_timer();
+  EXPECT_FALSE(ret);
+  EXPECT_EQ(0u, t_runs);
+}
+
+TEST_F(TestTimersManager, timers_order)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t1_runs = 0;
+  auto t1 = TimerT::make_shared(
+    10ms,
+    [&t1_runs]() {
+      t1_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t2_runs = 0;
+  auto t2 = TimerT::make_shared(
+    30ms,
+    [&t2_runs]() {
+      t2_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t3_runs = 0;
+  auto t3 = TimerT::make_shared(
+    100ms,
+    [&t3_runs]() {
+      t3_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  // Add timers in a random order
+  timers_manager->add_timer(t2);
+  timers_manager->add_timer(t3);
+  timers_manager->add_timer(t1);
+
+  std::this_thread::sleep_for(10ms);
+  execute_all_ready_timers(timers_manager);
+  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);
+  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);
+  EXPECT_EQ(3u, t1_runs);
+  EXPECT_EQ(2u, t2_runs);
+  EXPECT_EQ(1u, t3_runs);
+
+  timers_manager->remove_timer(t1);
+
+  std::this_thread::sleep_for(30ms);
+  execute_all_ready_timers(timers_manager);
+  EXPECT_EQ(3u, t1_runs);
+  EXPECT_EQ(3u, t2_runs);
+  EXPECT_EQ(1u, t3_runs);
+}
+
+TEST_F(TestTimersManager, start_stop_timers_thread)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  auto t = TimerT::make_shared(1ms, []() {}, rclcpp::contexts::get_global_default_context());
+  timers_manager->add_timer(t);
+
+  // Calling start multiple times will throw an error
+  EXPECT_NO_THROW(timers_manager->start());
+  EXPECT_THROW(timers_manager->start(), std::exception);
+
+  // Calling stop multiple times does not throw an error
+  EXPECT_NO_THROW(timers_manager->stop());
+  EXPECT_NO_THROW(timers_manager->stop());
+}
+
+TEST_F(TestTimersManager, timers_thread)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t1_runs = 0;
+  auto t1 = TimerT::make_shared(
+    1ms,
+    [&t1_runs]() {
+      t1_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t2_runs = 0;
+  auto t2 = TimerT::make_shared(
+    1ms,
+    [&t2_runs]() {
+      t2_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  // Add timers
+  timers_manager->add_timer(t1);
+  timers_manager->add_timer(t2);
+
+  // Run timers thread for a while
+  timers_manager->start();
+  std::this_thread::sleep_for(5ms);
+  timers_manager->stop();
+
+  EXPECT_LT(1u, t1_runs);
+  EXPECT_LT(1u, t2_runs);
+  EXPECT_EQ(t1_runs, t2_runs);
+}
+
+TEST_F(TestTimersManager, destructor)
+{
+  size_t t_runs = 0;
+  auto t = TimerT::make_shared(
+    1ms,
+    [&t_runs]() {
+      t_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+  std::weak_ptr<TimerT> t_weak = t;
+
+  // When the timers manager is destroyed, it will stop the thread
+  // and clear the timers
+  {
+    auto timers_manager = std::make_shared<TimersManager>(
+      rclcpp::contexts::get_global_default_context());
+
+    timers_manager->add_timer(t);
+
+    timers_manager->start();
+    std::this_thread::sleep_for(100ms);
+
+    EXPECT_LT(1u, t_runs);
+  }
+
+  // The thread is not running anymore, so this value does not increase
+  size_t runs = t_runs;
+  std::this_thread::sleep_for(100ms);
+  EXPECT_EQ(runs, t_runs);
+  t.reset();
+  EXPECT_FALSE(t_weak.lock() != nullptr);
+}
+
+TEST_F(TestTimersManager, add_remove_while_thread_running)
+{
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t1_runs = 0;
+  auto t1 = TimerT::make_shared(
+    1ms,
+    [&t1_runs]() {
+      t1_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t2_runs = 0;
+  auto t2 = TimerT::make_shared(
+    1ms,
+    [&t2_runs]() {
+      t2_runs++;
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  // Add timers
+  timers_manager->add_timer(t1);
+
+  // Start timers thread
+  timers_manager->start();
+
+  // After a while remove t1 and add t2
+  std::this_thread::sleep_for(5ms);
+  timers_manager->remove_timer(t1);
+  size_t tmp_t1 = t1_runs;
+  timers_manager->add_timer(t2);
+
+  // Wait some more time and then stop
+  std::this_thread::sleep_for(5ms);
+  timers_manager->stop();
+
+  // t1 has stopped running
+  EXPECT_EQ(tmp_t1, t1_runs);
+  // t2 is correctly running
+  EXPECT_LT(1u, t2_runs);
+}
+
+TEST_F(TestTimersManager, infinite_loop)
+{
+  // This test makes sure that even if timers have a period shorter than the duration
+  // of their callback the functions never block indefinitely.
+
+  auto timers_manager = std::make_shared<TimersManager>(
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t1_runs = 0;
+  auto t1 = TimerT::make_shared(
+    1ms,
+    [&t1_runs]() {
+      t1_runs++;
+      std::this_thread::sleep_for(5ms);
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  size_t t2_runs = 0;
+  auto t2 = TimerT::make_shared(
+    1ms,
+    [&t2_runs]() {
+      t2_runs++;
+      std::this_thread::sleep_for(5ms);
+    },
+    rclcpp::contexts::get_global_default_context());
+
+  timers_manager->add_timer(t1);
+  timers_manager->add_timer(t2);
+
+  // Start a timers thread and make sure that we can stop it later
+  timers_manager->start();
+  std::this_thread::sleep_for(50ms);
+  timers_manager->stop();
+
+  EXPECT_LT(0u, t1_runs);
+  EXPECT_LT(0u, t2_runs);
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,40 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+21.0.7 (2024-07-10)
+-------------------
+* fix: Fixed race condition in action server between is_ready and take"… (`#2531 <https://github.com/ros2/rclcpp/issues/2531>`_)
+* Do not generate the exception when action service response timeout. (`#2519 <https://github.com/ros2/rclcpp/issues/2519>`_)
+* Contributors: Janosch Machowinski, Tomoya Fujita, William Woodall
+
+21.0.6 (2024-04-19)
+-------------------
+
+21.0.5 (2024-02-07)
+-------------------
+
+21.0.4 (2023-11-17)
+-------------------
+
+21.0.3 (2023-09-08)
+-------------------
+
+21.0.2 (2023-07-14)
+-------------------
+
+21.0.1 (2023-05-11)
+-------------------
+
+21.0.0 (2023-04-18)
+-------------------
+
+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>19.3.0</version>
+  <version>21.0.7</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_action/src/client.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>
 
 #include "rcl_action/action_client.h"
 #include "rcl_action/wait.h"
@@ -31,6 +32,67 @@
 namespace rclcpp_action
 {
 
+struct ClientBaseData
+{
+  struct FeedbackReadyData
+  {
+    FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), feedback_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> feedback_message;
+  };
+  struct StatusReadyData
+  {
+    StatusReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), status_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> status_message;
+  };
+  struct GoalResponseData
+  {
+    GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), goal_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response;
+  };
+  struct CancelResponseData
+  {
+    CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), cancel_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response;
+  };
+  struct ResultResponseData
+  {
+    ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), result_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response;
+  };
+
+  std::variant<
+    FeedbackReadyData,
+    StatusReadyData,
+    GoalResponseData,
+    CancelResponseData,
+    ResultResponseData
+  > data;
+
+  explicit ClientBaseData(FeedbackReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(StatusReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(GoalResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(CancelResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(ResultResponseData && data_in)
+  : data(std::move(data_in)) {}
+};
+
 class ClientBaseImpl
 {
 public:
@@ -94,11 +156,13 @@ public:
   size_t num_clients{0u};
   size_t num_services{0u};
 
-  bool is_feedback_ready{false};
-  bool is_status_ready{false};
-  bool is_goal_response_ready{false};
-  bool is_cancel_response_ready{false};
-  bool is_result_response_ready{false};
+  // Lock for action_client_
+  std::recursive_mutex action_client_mutex_;
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
@@ -142,6 +206,7 @@ bool
 ClientBase::action_server_is_ready() const
 {
   bool is_ready;
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_server_is_available(
     this->pimpl_->node_handle.get(),
     this->pimpl_->client_handle.get(),
@@ -255,6 +320,7 @@ ClientBase::get_number_of_ready_services()
 void
 ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_client(
     wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
   if (RCL_RET_OK != ret) {
@@ -265,23 +331,56 @@ ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 bool
 ClientBase::is_ready(rcl_wait_set_t * wait_set)
 {
-  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
-    wait_set, pimpl_->client_handle.get(),
-    &pimpl_->is_feedback_ready,
-    &pimpl_->is_status_ready,
-    &pimpl_->is_goal_response_ready,
-    &pimpl_->is_cancel_response_ready,
-    &pimpl_->is_result_response_ready);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "failed to check for any ready entities");
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+    ret = rcl_action_client_wait_set_get_entities_ready(
+      wait_set, pimpl_->client_handle.get(),
+      &is_feedback_ready,
+      &is_status_ready,
+      &is_goal_response_ready,
+      &is_cancel_response_ready,
+      &is_result_response_ready);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to check for any ready entities");
+    }
   }
-  return
-    pimpl_->is_feedback_ready ||
-    pimpl_->is_status_ready ||
-    pimpl_->is_goal_response_ready ||
-    pimpl_->is_cancel_response_ready ||
-    pimpl_->is_result_response_ready;
+
+  pimpl_->next_ready_event = ClientBaseImpl::NO_EVENT_READY;
+
+  if (is_feedback_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::FeedbackSubscription);
+    return true;
+  }
+
+  if (is_status_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::StatusSubscription);
+    return true;
+  }
+
+  if (is_goal_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::GoalClient);
+    return true;
+  }
+
+  if (is_result_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::ResultClient);
+    return true;
+  }
+
+  if (is_cancel_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::CancelClient);
+    return true;
+  }
+
+  return false;
 }
 
 void
@@ -432,7 +531,6 @@ ClientBase::set_callback_to_entity(
       }
     };
 
-
   // Set it temporarily to the new callback, while we replace the old one.
   // This two-step setting, prevents a gap where the old std::function has
   // been replaced but the middleware hasn't been told about the new one yet.
@@ -550,140 +648,159 @@ ClientBase::clear_on_ready_callback()
 std::shared_ptr<void>
 ClientBase::take_data()
 {
-  if (pimpl_->is_feedback_ready) {
-    std::shared_ptr<void> feedback_message = this->create_feedback_message();
-    rcl_ret_t ret = rcl_action_take_feedback(
-      pimpl_->client_handle.get(), feedback_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, feedback_message));
-  } else if (pimpl_->is_status_ready) {
-    std::shared_ptr<void> status_message = this->create_status_message();
-    rcl_ret_t ret = rcl_action_take_status(
-      pimpl_->client_handle.get(), status_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, status_message));
-  } else if (pimpl_->is_goal_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> goal_response = this->create_goal_response();
-    rcl_ret_t ret = rcl_action_take_goal_response(
-      pimpl_->client_handle.get(), &response_header, goal_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, goal_response));
-  } else if (pimpl_->is_result_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> result_response = this->create_result_response();
-    rcl_ret_t ret = rcl_action_take_result_response(
-      pimpl_->client_handle.get(), &response_header, result_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, result_response));
-  } else if (pimpl_->is_cancel_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> cancel_response = this->create_cancel_response();
-    rcl_ret_t ret = rcl_action_take_cancel_response(
-      pimpl_->client_handle.get(), &response_header, cancel_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, cancel_response));
-  } else {
-    throw std::runtime_error("Taking data from action client but nothing is ready");
+  // next_ready_event is an atomic, caching localy
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ClientBaseImpl::NO_EVENT_READY);
+
+  if (next_ready_event == ClientBaseImpl::NO_EVENT_READY) {
+    // there is a known bug in iron, that take_data might be called multiple
+    // times. Therefore instead of throwing, we just return a nullptr as a workaround.
+    return nullptr;
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ClientBase::take_data_by_entity_id(size_t id)
 {
+  std::shared_ptr<ClientBaseData> data_ptr;
+  rcl_ret_t ret;
+
   // Mark as ready the entity from which we want to take data
   switch (static_cast<EntityType>(id)) {
     case EntityType::GoalClient:
-      pimpl_->is_goal_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> goal_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+
+          goal_response = this->create_goal_response();
+          ret = rcl_action_take_goal_response(
+            pimpl_->client_handle.get(), &response_header, goal_response.get());
+        }
+        data_ptr = std::make_shared<ClientBaseData>(
+          ClientBaseData::GoalResponseData(
+            ret, response_header, goal_response));
+      }
       break;
     case EntityType::ResultClient:
-      pimpl_->is_result_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> result_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          result_response = this->create_result_response();
+          ret = rcl_action_take_result_response(
+            pimpl_->client_handle.get(), &response_header, result_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::ResultResponseData(
+            ret, response_header, result_response));
+      }
       break;
     case EntityType::CancelClient:
-      pimpl_->is_cancel_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> cancel_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          cancel_response = this->create_cancel_response();
+          ret = rcl_action_take_cancel_response(
+            pimpl_->client_handle.get(), &response_header, cancel_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::CancelResponseData(
+            ret, response_header, cancel_response));
+      }
       break;
     case EntityType::FeedbackSubscription:
-      pimpl_->is_feedback_ready = true;
+      {
+        std::shared_ptr<void> feedback_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          feedback_message = this->create_feedback_message();
+          ret = rcl_action_take_feedback(
+            pimpl_->client_handle.get(), feedback_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::FeedbackReadyData(
+            ret, feedback_message));
+      }
       break;
     case EntityType::StatusSubscription:
-      pimpl_->is_status_ready = true;
+      {
+        std::shared_ptr<void> status_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          status_message = this->create_status_message();
+          ret = rcl_action_take_status(
+            pimpl_->client_handle.get(), status_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::StatusReadyData(
+            ret, status_message));
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ClientBase::execute(std::shared_ptr<void> & data)
+ClientBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    // workaround, if take_data was called multiple timed, it returns a nullptr
+    // normally we should throw here, but as an API stable bug fix, we just ignore this...
+    return;
   }
 
-  if (pimpl_->is_feedback_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_feedback_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto feedback_message = std::get<1>(*shared_ptr);
-      this->handle_feedback_message(feedback_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
-    }
-  } else if (pimpl_->is_status_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_status_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto status_message = std::get<1>(*shared_ptr);
-      this->handle_status_message(status_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
-    }
-  } else if (pimpl_->is_goal_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_goal_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto goal_response = std::get<2>(*shared_ptr);
-      this->handle_goal_response(response_header, goal_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
-    }
-  } else if (pimpl_->is_result_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_result_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto result_response = std::get<2>(*shared_ptr);
-      this->handle_result_response(response_header, result_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
-    }
-  } else if (pimpl_->is_cancel_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_cancel_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto cancel_response = std::get<2>(*shared_ptr);
-      this->handle_cancel_response(response_header, cancel_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
-    }
-  } else {
-    throw std::runtime_error("Executing action client but nothing is ready");
-  }
+  std::shared_ptr<ClientBaseData> data_ptr = std::static_pointer_cast<ClientBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ClientBaseData::FeedbackReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_feedback_message(data.feedback_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::StatusReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_status_message(data.status_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::GoalResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_goal_response(data.response_header, data.goal_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::ResultResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_result_response(data.response_header, data.result_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::CancelResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_cancel_response(data.response_header, data.cancel_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response");
+        }
+      }
+    }, data_ptr->data);
 }
 
 }  // namespace rclcpp_action

rclcpp_action/src/server.cpp

@@ -18,6 +18,7 @@
 #include <tuple>
 #include <unordered_map>
 #include <utility>
+#include <variant>
 #include <vector>
 
 #include "rcl_action/action_server.h"
@@ -33,8 +34,50 @@
 using rclcpp_action::ServerBase;
 using rclcpp_action::GoalUUID;
 
+struct ServerBaseData;
+
 namespace rclcpp_action
 {
+
+struct ServerBaseData
+{
+  using GoalRequestData = std::tuple<
+    rcl_ret_t,
+    const rcl_action_goal_info_t,
+    rmw_request_id_t,
+    std::shared_ptr<void>
+  >;
+
+  using CancelRequestData = std::tuple<
+    rcl_ret_t,
+    std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
+    rmw_request_id_t
+  >;
+
+  using ResultRequestData = std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>;
+
+  using GoalExpiredData = struct Empty {};
+
+  std::variant<GoalRequestData, CancelRequestData, ResultRequestData, GoalExpiredData> data;
+
+  explicit ServerBaseData(GoalRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(CancelRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(ResultRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(GoalExpiredData && data_in)
+  : data(std::move(data_in)) {}
+};
+
+enum class ActionEventType : std::size_t
+{
+  GoalService,
+  ResultService,
+  CancelService,
+  Expired,
+};
+
 class ServerBaseImpl
 {
 public:
@@ -60,11 +103,6 @@ public:
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  std::atomic<bool> goal_request_ready_{false};
-  std::atomic<bool> cancel_request_ready_{false};
-  std::atomic<bool> result_request_ready_{false};
-  std::atomic<bool> goal_expired_{false};
-
   // Lock for unordered_maps
   std::recursive_mutex unordered_map_mutex_;
 
@@ -75,8 +113,15 @@ public:
   // rcl goal handles are kept so api to send result doesn't try to access freed memory
   std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
 
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
+
   rclcpp::Logger logger_;
 };
+
 }  // namespace rclcpp_action
 
 ServerBase::ServerBase(
@@ -194,124 +239,170 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set)
       &goal_expired);
   }
 
-  pimpl_->goal_request_ready_ = goal_request_ready;
-  pimpl_->cancel_request_ready_ = cancel_request_ready;
-  pimpl_->result_request_ready_ = result_request_ready;
-  pimpl_->goal_expired_ = goal_expired;
-
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_.load() ||
-         pimpl_->cancel_request_ready_.load() ||
-         pimpl_->result_request_ready_.load() ||
-         pimpl_->goal_expired_.load();
+  pimpl_->next_ready_event = ServerBaseImpl::NO_EVENT_READY;
+
+  if (goal_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::GoalService);
+    return true;
+  }
+
+  if (cancel_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::CancelService);
+    return true;
+  }
+
+  if (result_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::ResultService);
+    return true;
+  }
+
+  if (goal_expired) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::Expired);
+    return true;
+  }
+
+  return false;
 }
 
 std::shared_ptr<void>
 ServerBase::take_data()
 {
-  if (pimpl_->goal_request_ready_.load()) {
-    rcl_ret_t ret;
-    rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
-    rmw_request_id_t request_header;
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ServerBaseImpl::NO_EVENT_READY);
 
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-
-    std::shared_ptr<void> message = create_goal_request();
-    ret = rcl_action_take_goal_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      message.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret,
-        goal_info,
-        request_header, message));
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    rcl_ret_t ret;
-    rmw_request_id_t request_header;
-
-    // Initialize cancel request
-    auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
-
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_cancel_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(ret, request, request_header));
-  } else if (pimpl_->result_request_ready_.load()) {
-    rcl_ret_t ret;
-    // Get the result request message
-    rmw_request_id_t request_header;
-    std::shared_ptr<void> result_request = create_result_request();
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_result_request(
-      pimpl_->action_server_.get(), &request_header, result_request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(
-        ret, result_request, request_header));
-  } else if (pimpl_->goal_expired_.load()) {
+  if (next_ready_event == ServerBaseImpl::NO_EVENT_READY) {
+    // there is a known bug in iron, that take_data might be called multiple
+    // times. Therefore instead of throwing, we just return a nullptr as a workaround.
     return nullptr;
-  } else {
-    throw std::runtime_error("Taking data from action server but nothing is ready");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ServerBase::take_data_by_entity_id(size_t id)
 {
+  static_assert(
+    static_cast<size_t>(EntityType::GoalService) ==
+    static_cast<size_t>(ActionEventType::GoalService));
+  static_assert(
+    static_cast<size_t>(EntityType::ResultService) ==
+    static_cast<size_t>(ActionEventType::ResultService));
+  static_assert(
+    static_cast<size_t>(EntityType::CancelService) ==
+    static_cast<size_t>(ActionEventType::CancelService));
+
+  std::shared_ptr<ServerBaseData> data_ptr;
   // Mark as ready the entity from which we want to take data
-  switch (static_cast<EntityType>(id)) {
-    case EntityType::GoalService:
-      pimpl_->goal_request_ready_ = true;
+  switch (static_cast<ActionEventType>(id)) {
+    case ActionEventType::GoalService:
+      {
+        rcl_ret_t ret;
+        rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+        rmw_request_id_t request_header;
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
+        std::shared_ptr<void> message = create_goal_request();
+        ret = rcl_action_take_goal_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          message.get());
+
+        data_ptr = std::make_shared<ServerBaseData>(
+          ServerBaseData::GoalRequestData(ret, goal_info, request_header, message));
+      }
       break;
-    case EntityType::ResultService:
-      pimpl_->result_request_ready_ = true;
+    case ActionEventType::ResultService:
+      {
+        rcl_ret_t ret;
+        // Get the result request message
+        rmw_request_id_t request_header;
+        std::shared_ptr<void> result_request = create_result_request();
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_result_request(
+          pimpl_->action_server_.get(), &request_header, result_request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::ResultRequestData(ret, result_request, request_header));
+      }
       break;
-    case EntityType::CancelService:
-      pimpl_->cancel_request_ready_ = true;
+    case ActionEventType::CancelService:
+      {
+        rcl_ret_t ret;
+        rmw_request_id_t request_header;
+
+        // Initialize cancel request
+        auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_cancel_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::CancelRequestData(ret, request, request_header));
+      }
+      break;
+    case ActionEventType::Expired:
+      {
+        data_ptr =
+          std::make_shared<ServerBaseData>(ServerBaseData::GoalExpiredData());
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ServerBase::execute(std::shared_ptr<void> & data)
+ServerBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data && !pimpl_->goal_expired_.load()) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    // workaround, if take_data was called multiple timed, it returns a nullptr
+    // normally we should throw here, but as an API stable bug fix, we just ignore this...
+    return;
   }
 
-  if (pimpl_->goal_request_ready_.load()) {
-    execute_goal_request_received(data);
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    execute_cancel_request_received(data);
-  } else if (pimpl_->result_request_ready_.load()) {
-    execute_result_request_received(data);
-  } else if (pimpl_->goal_expired_.load()) {
-    execute_check_expired_goals();
-  } else {
-    throw std::runtime_error("Executing action server but nothing is ready");
-  }
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalRequestData>) {
+        execute_goal_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::CancelRequestData>) {
+        execute_cancel_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::ResultRequestData>) {
+        execute_result_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalExpiredData>) {
+        execute_check_expired_goals();
+      }
+    },
+    data_ptr->data);
 }
 
 void
 ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-  rcl_ret_t ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::GoalRequestData & gData(
+    std::get<ServerBaseData::GoalRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  rcl_action_goal_info_t goal_info = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+  const std::shared_ptr<void> message = std::get<3>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -320,14 +411,6 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr);
-  rmw_request_id_t request_header = std::get<2>(*shared_ptr);
-  std::shared_ptr<void> message = std::get<3>(*shared_ptr);
-
-  bool expected = true;
-  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
-    return;
-  }
 
   GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
@@ -344,7 +427,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   }
 
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send goal response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
   }
 
   const auto status = response_pair.first;
@@ -402,10 +494,15 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::CancelRequestData & gData(
+    std::get<ServerBaseData::CancelRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<action_msgs::srv::CancelGoal::Request> request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -414,9 +511,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
-  pimpl_->cancel_request_ready_ = false;
 
   // Convert c++ message to C message
   rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
@@ -483,6 +577,15 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
       pimpl_->action_server_.get(), &request_header, response.get());
   }
 
+  if (ret == RCL_RET_TIMEOUT) {
+    GoalUUID uuid = request->goal_info.goal_id.uuid;
+    RCLCPP_WARN(
+      pimpl_->logger_,
+      "Failed to send cancel response %s (timeout): %s",
+      to_string(uuid).c_str(), rcl_get_error_string().str);
+    rcl_reset_error();
+    return;
+  }
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -492,9 +595,14 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::ResultRequestData & gData(
+    std::get<ServerBaseData::ResultRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<void> result_request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -503,10 +611,7 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto result_request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
 
-  pimpl_->result_request_ready_ = false;
   std::shared_ptr<void> result_response;
 
   // check if the goal exists
@@ -538,6 +643,14 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     rcl_ret_t rcl_ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (rcl_ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send result response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (RCL_RET_OK != rcl_ret) {
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
@@ -671,7 +784,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
       for (auto & request_header : iter->second) {
         rcl_ret_t ret = rcl_action_send_result_response(
           pimpl_->action_server_.get(), &request_header, result_msg.get());
-        if (RCL_RET_OK != ret) {
+        if (ret == RCL_RET_TIMEOUT) {
+          RCLCPP_WARN(
+            pimpl_->logger_,
+            "Failed to send result response %s (timeout): %s",
+            to_string(uuid).c_str(), rcl_get_error_string().str);
+          rcl_reset_error();
+        } else if (RCL_RET_OK != ret) {
           rclcpp::exceptions::throw_from_rcl_error(ret);
         }
       }

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,39 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+21.0.7 (2024-07-10)
+-------------------
+
+21.0.6 (2024-04-19)
+-------------------
+
+21.0.5 (2024-02-07)
+-------------------
+* Increase the service queue sizes in component_container (`#2381 <https://github.com/ros2/rclcpp/issues/2381>`_)
+* Contributors: M. Fatih Cırıt
+
+21.0.4 (2023-11-17)
+-------------------
+* Add missing header required by the rclcpp::NodeOptions type (`#2325 <https://github.com/ros2/rclcpp/issues/2325>`_)
+* Contributors: Ignacio Vizzo
+
+21.0.3 (2023-09-08)
+-------------------
+
+21.0.2 (2023-07-14)
+-------------------
+
+21.0.1 (2023-05-11)
+-------------------
+
+21.0.0 (2023-04-18)
+-------------------
+
+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/include/rclcpp_components/node_factory.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 #define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 
+#include "rclcpp/node_options.hpp"
 #include "rclcpp_components/node_instance_wrapper.hpp"
 
 namespace rclcpp_components

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>19.3.0</version>
+  <version>21.0.7</version>
   <description>Package containing tools for dynamically loadable components</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_components/src/component_manager.cpp

@@ -39,10 +39,12 @@ ComponentManager::ComponentManager(
 {
   loadNode_srv_ = create_service<LoadNode>(
     "~/_container/load_node",
-    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3),
+    rclcpp::ServicesQoS().keep_last(200));
   unloadNode_srv_ = create_service<UnloadNode>(
     "~/_container/unload_node",
-    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3),
+    rclcpp::ServicesQoS().keep_last(200));
   listNodes_srv_ = create_service<ListNodes>(
     "~/_container/list_nodes",
     std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3));