make timeouts and sleeps more robust

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

CODEOWNERS

@@ -0,0 +1,2 @@
+# This file was generated by https://github.com/audrow/update-ros2-repos
+* @ivanpauno @hidmic @wjwwood

README.md

@@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+
 
 ### Examples
 

rclcpp/CHANGELOG.rst

@@ -2,6 +2,67 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+19.3.0 (2023-03-01)
+-------------------
+* Fix memory leak in tracetools::get_symbol() (`#2104 <https://github.com/ros2/rclcpp/issues/2104>`_)
+* Service introspection (`#1985 <https://github.com/ros2/rclcpp/issues/1985>`_)
+* Allow publishing borrowed messages with intra-process enabled (`#2108 <https://github.com/ros2/rclcpp/issues/2108>`_)
+* to fix flaky test about TestTimeSource.callbacks (`#2111 <https://github.com/ros2/rclcpp/issues/2111>`_)
+* Contributors: Brian, Chen Lihui, Christophe Bedard, Miguel Company
+
+19.2.0 (2023-02-24)
+-------------------
+* to create a sublogger while getting child of Logger (`#1717 <https://github.com/ros2/rclcpp/issues/1717>`_)
+* Fix documentation of Context class (`#2107 <https://github.com/ros2/rclcpp/issues/2107>`_)
+* fixes for rmw callbacks in qos_event class (`#2102 <https://github.com/ros2/rclcpp/issues/2102>`_)
+* Contributors: Alberto Soragna, Chen Lihui, Silvio Traversaro
+
+19.1.0 (2023-02-14)
+-------------------
+* Add support for timers on reset callback (`#1979 <https://github.com/ros2/rclcpp/issues/1979>`_)
+* Topic node guard condition in executor (`#2074 <https://github.com/ros2/rclcpp/issues/2074>`_)
+* Fix bug on the disorder of calling shutdown callback (`#2097 <https://github.com/ros2/rclcpp/issues/2097>`_)
+* Contributors: Barry Xu, Chen Lihui, mauropasse
+
+19.0.0 (2023-01-30)
+-------------------
+* Add default constructor to NodeInterfaces (`#2094 <https://github.com/ros2/rclcpp/issues/2094>`_)
+* Fix clock state cached time to be a copy, not a reference. (`#2092 <https://github.com/ros2/rclcpp/issues/2092>`_)
+* Fix -Wmaybe-uninitialized warning (`#2081 <https://github.com/ros2/rclcpp/issues/2081>`_)
+* Fix the keep_last warning when using system defaults. (`#2082 <https://github.com/ros2/rclcpp/issues/2082>`_)
+* Add in a fix for older compilers. (`#2075 <https://github.com/ros2/rclcpp/issues/2075>`_)
+* Contributors: Alexander Hans, Chris Lalancette, Shane Loretz
+
+18.0.0 (2022-12-29)
+-------------------
+* Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)
+* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_)
+* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 <https://github.com/ros2/rclcpp/issues/2066>`_)
+* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_)
+* Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_)
+* Add clock type to node_options (`#1982 <https://github.com/ros2/rclcpp/issues/1982>`_)
+* Fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_)
+* Remove templating on to_rcl_subscription_options (`#2056 <https://github.com/ros2/rclcpp/issues/2056>`_)
+* Fix SharedFuture from async_send_request never becoming valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_)
+* Add in a warning for a KeepLast depth of 0. (`#2048 <https://github.com/ros2/rclcpp/issues/2048>`_)
+* Mark rclcpp::Clock::now() as const (`#2050 <https://github.com/ros2/rclcpp/issues/2050>`_)
+* Fix a case that did not throw ParameterUninitializedException (`#2036 <https://github.com/ros2/rclcpp/issues/2036>`_)
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon
+
+17.1.0 (2022-11-02)
+-------------------
+* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 <https://github.com/ros2/rclcpp/issues/2032>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_)
+* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 <https://github.com/ros2/rclcpp/issues/2030>`_)
+* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_)
+* Set cpplint test timeout to 3 minutes (`#2022 <https://github.com/ros2/rclcpp/issues/2022>`_)
+* Make sure to include-what-you-use in the node_interfaces. (`#2018 <https://github.com/ros2/rclcpp/issues/2018>`_)
+* Do not clear entities callbacks on destruction (`#2002 <https://github.com/ros2/rclcpp/issues/2002>`_)
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_)
+* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks
+
 17.0.0 (2022-09-13)
 -------------------
 * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 <https://github.com/ros2/rclcpp/issues/1978>`_)

rclcpp/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(tracetools REQUIRED)
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
@@ -50,6 +51,9 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions/exceptions.cpp
+  src/rclcpp/experimental/executors/events_executor/events_executor_entities_collector.cpp
+  src/rclcpp/experimental/executors/events_executor/events_executor.cpp
+  src/rclcpp/experimental/timers_manager.cpp
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
@@ -92,7 +96,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/parameter_value.cpp
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
-  src/rclcpp/qos_event.cpp
+  src/rclcpp/event_handler.cpp
   src/rclcpp/qos_overriding_options.cpp
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp
@@ -259,4 +263,8 @@ if(TEST cppcheck)
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
 
+if(TEST cpplint)
+  set_tests_properties(cpplint PROPERTIES TIMEOUT 180)
+endif()
+
 ament_generate_version_header(${PROJECT_NAME})

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 
+#include <cstring>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
+template<typename Alloc>
+void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  size_t size = number_of_elem * size_of_elem;
+  void * allocated_memory =
+    std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+  if (allocated_memory) {
+    std::memset(allocated_memory, 0, size);
+  }
+  return allocated_memory;
+}
+
 template<typename T, typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
@@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
   rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.zero_allocate = &retyped_zero_allocate<Alloc>;
   rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
   rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
   rcl_allocator.state = &allocator;

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -191,10 +191,14 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && arg) {
-        TRACEPOINT(
-          rclcpp_callback_register,
-          static_cast<const void *>(this),
-          tracetools::get_symbol(arg));
+        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(arg);
+          DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
       }, callback_);
 #endif  // TRACETOOLS_DISABLED
   }

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -965,10 +965,14 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && callback) {
-        TRACEPOINT(
-          rclcpp_callback_register,
-          static_cast<const void *>(this),
-          tracetools::get_symbol(callback));
+        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(callback);
+          DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
       }, callback_variant_);
 #endif  // TRACETOOLS_DISABLED
   }

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -95,6 +98,10 @@ public:
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  ~CallbackGroup();
+
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
   find_subscription_ptrs_if(Function func) const
@@ -171,6 +178,16 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
+  /// Defer creating the notify guard condition and return it.
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -213,6 +230,9 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -16,14 +16,15 @@
 #define RCLCPP__CLIENT_HPP_
 
 #include <atomic>
+#include <functional>
 #include <future>
-#include <unordered_map>
 #include <memory>
 #include <mutex>
 #include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
 #include <tuple>
+#include <unordered_map>
 #include <utility>
 #include <variant>  // NOLINT
 #include <vector>
@@ -31,8 +32,10 @@
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
+#include "rcl/service_introspection.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -130,7 +133,7 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
-  virtual ~ClientBase();
+  virtual ~ClientBase() = default;
 
   /// Take the next response for this client as a type erased pointer.
   /**
@@ -312,7 +315,7 @@ public:
     // 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.
     set_on_new_response_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -320,7 +323,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_response_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_response_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_response_callback_));
   }
 
@@ -466,15 +470,13 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph)
+  : ClientBase(node_base, node_graph),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle =
-      get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
       this->get_client_handle().get(),
       this->get_rcl_node_handle(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &client_options);
     if (ret != RCL_RET_OK) {
@@ -769,7 +771,9 @@ public:
     auto old_size = pending_requests_.size();
     for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
       if (it->second.first < time_point) {
-        pruned_requests->push_back(it->first);
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
         it = pending_requests_.erase(it);
       } else {
         ++it;
@@ -778,6 +782,33 @@ public:
     return old_size - pending_requests_.size();
   }
 
+  /// Configure client 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
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_client_configure_service_introspection(
+      client_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection");
+    }
+  }
+
 protected:
   using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
   using CallbackWithRequestTypeValueVariant = std::tuple<
@@ -792,16 +823,14 @@ protected:
   async_send_request_impl(const Request & request, CallbackInfoVariant value)
   {
     int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
-    {
-      std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-      pending_requests_.try_emplace(
-        sequence_number,
-        std::make_pair(std::chrono::system_clock::now(), std::move(value)));
-    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
     return sequence_number;
   }
 
@@ -830,6 +859,9 @@ protected:
       CallbackInfoVariant>>
   pending_requests_;
   std::mutex pending_requests_mutex_;
+
+private:
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/clock.hpp

@@ -77,7 +77,7 @@ public:
    */
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
 
   /**
    * Sleep until a specified Time, according to clock type.
@@ -137,6 +137,51 @@ public:
     Duration rel_time,
     Context::SharedPtr context = contexts::get_global_default_context());
 
+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/context.hpp

@@ -65,8 +65,11 @@ using PreShutdownCallbackHandle = ShutdownCallbackHandle;
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
- * It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
- * and rclcpp::shutdown.
+ * Nodes may be attached to a particular context by passing to the rclcpp::Node
+ * constructor a rclcpp::NodeOptions instance in which the Context is set via
+ * rclcpp::NodeOptions::context.
+ * Nodes will be automatically removed from the context when destructed.
+ * Contexts may be shutdown by calling rclcpp::shutdown.
  */
 class Context : public std::enable_shared_from_this<Context>
 {
@@ -376,10 +379,10 @@ private:
   // attempt to acquire another sub context.
   std::recursive_mutex sub_contexts_mutex_;
 
-  std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
-  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
   mutable std::mutex pre_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).
@@ -398,20 +401,22 @@ private:
 
   using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
 
+  template<ShutdownType shutdown_type>
   RCLCPP_LOCAL
   ShutdownCallbackHandle
   add_shutdown_callback(
-    ShutdownType shutdown_type,
     ShutdownCallback callback);
 
+  template<ShutdownType shutdown_type>
   RCLCPP_LOCAL
   bool
   remove_shutdown_callback(
-    ShutdownType shutdown_type,
     const ShutdownCallbackHandle & callback_handle);
 
+  template<ShutdownType shutdown_type>
+  RCLCPP_LOCAL
   std::vector<rclcpp::Context::ShutdownCallback>
-  get_shutdown_callback(ShutdownType shutdown_type) const;
+  get_shutdown_callback() const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -41,6 +41,7 @@ namespace detail
  * so no exceptions should be thrown at this point, and doing so results in
  * undefined behavior.
  *
+ * \tparam UserDataRealT Declared type of the passed function
  * \tparam UserDataT Deduced type based on what is passed for user data,
  *   usually this type is either `void *` or `const void *`.
  * \tparam Args the arguments being passed to the callback
@@ -50,6 +51,7 @@ namespace detail
  * \returns whatever the callback returns, if anything
  */
 template<
+  typename UserDataRealT,
   typename UserDataT,
   typename ... Args,
   typename ReturnT = void
@@ -57,7 +59,7 @@ template<
 ReturnT
 cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
 {
-  auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
+  auto & actual_callback = *static_cast<const UserDataRealT *>(user_data);
   return actual_callback(args ...);
 }
 

rclcpp/include/rclcpp/detail/template_contains.hpp

@@ -0,0 +1,47 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_CONTAINS_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+
+#include <type_traits>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given T is contained in the list Us.
+template<typename T, typename ... Us>
+struct template_contains;
+
+template<typename ... Args>
+inline constexpr bool template_contains_v = template_contains<Args ...>::value;
+
+template<typename T, typename NextT, typename ... Us>
+struct template_contains<T, NextT, Us ...>
+{
+  enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
+};
+
+template<typename T>
+struct template_contains<T>
+{
+  enum { value = false };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_

rclcpp/include/rclcpp/detail/template_unique.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_UNIQUE_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/detail/template_contains.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given list Ts contains unique types.
+template<typename ... Ts>
+struct template_unique;
+
+template<typename ... Args>
+inline constexpr bool template_unique_v = template_unique<Args ...>::value;
+
+template<typename NextT, typename ... Ts>
+struct template_unique<NextT, Ts ...>
+{
+  enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
+};
+
+template<typename T>
+struct template_unique<T>
+{
+  enum { value = true };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_

rclcpp/include/rclcpp/event_handler.hpp

@@ -0,0 +1,311 @@
+// Copyright 2019 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__EVENT_HANDLER_HPP_
+#define RCLCPP__EVENT_HANDLER_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rmw/impl/cpp/demangle.hpp"
+#include "rmw/incompatible_qos_events_statuses.h"
+#include "rmw/events_statuses/incompatible_type.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+
+using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
+using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
+using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
+using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
+using QOSMessageLostInfo = rmw_message_lost_status_t;
+using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
+using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
+
+using IncompatibleTypeInfo = rmw_incompatible_type_status_t;
+using MatchedInfo = rmw_matched_status_t;
+
+using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
+using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
+using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
+using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
+using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
+using QOSRequestedIncompatibleQoSCallbackType =
+  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
+
+using IncompatibleTypeCallbackType = std::function<void (IncompatibleTypeInfo &)>;
+using PublisherMatchedCallbackType = std::function<void (MatchedInfo &)>;
+using SubscriptionMatchedCallbackType = std::function<void (MatchedInfo &)>;
+
+/// Contains callbacks for various types of events a Publisher can receive from the middleware.
+struct PublisherEventCallbacks
+{
+  QOSDeadlineOfferedCallbackType deadline_callback;
+  QOSLivelinessLostCallbackType liveliness_callback;
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  PublisherMatchedCallbackType matched_callback;
+};
+
+/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
+struct SubscriptionEventCallbacks
+{
+  QOSDeadlineRequestedCallbackType deadline_callback;
+  QOSLivelinessChangedCallbackType liveliness_callback;
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
+  QOSMessageLostCallbackType message_lost_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  SubscriptionMatchedCallbackType matched_callback;
+};
+
+class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    const exceptions::RCLErrorBase & base_exc,
+    const std::string & prefix);
+};
+
+class EventHandlerBase : public Waitable
+{
+public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
+  RCLCPP_PUBLIC
+  virtual ~EventHandlerBase();
+
+  /// Get the number of ready events
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_events() override;
+
+  /// Add the Waitable to a wait set.
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Check if the Waitable is ready.
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  /// Set a callback to be called when each new event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_event_set_callback
+   * \sa rcl_event_set_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Event));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::EventHandlerBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::EventHandlerBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // 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.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_event_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_event_callback_), const void *, size_t>,
+      static_cast<const void *>(&on_new_event_callback_));
+  }
+
+  /// Unset the callback registered for new events, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
+
+  rcl_event_t event_handle_;
+  size_t wait_set_event_index_;
+};
+
+using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase;
+
+template<typename EventCallbackT, typename ParentHandleT>
+class EventHandler : public EventHandlerBase
+{
+public:
+  template<typename InitFuncT, typename EventTypeEnum>
+  EventHandler(
+    const EventCallbackT & callback,
+    InitFuncT init_func,
+    ParentHandleT parent_handle,
+    EventTypeEnum event_type)
+  : parent_handle_(parent_handle), event_callback_(callback)
+  {
+    event_handle_ = rcl_get_zero_initialized_event();
+    rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_UNSUPPORTED) {
+        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
+        rcl_reset_error();
+        throw exc;
+      } else {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
+      }
+    }
+  }
+
+  /// Take data so that the callback cannot be scheduled again
+  std::shared_ptr<void>
+  take_data() override
+  {
+    EventCallbackInfoT callback_info;
+    rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't take event info: %s", rcl_get_error_string().str);
+      return nullptr;
+    }
+    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
+  }
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
+  /// Execute any entities of the Waitable that are ready.
+  void
+  execute(std::shared_ptr<void> & data) override
+  {
+    if (!data) {
+      throw std::runtime_error("'data' is empty");
+    }
+    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
+    event_callback_(*callback_ptr);
+    callback_ptr.reset();
+  }
+
+private:
+  using EventCallbackInfoT = typename std::remove_reference<typename
+      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
+
+  ParentHandleT parent_handle_;
+  EventCallbackT event_callback_;
+};
+
+template<typename EventCallbackT, typename ParentHandleT>
+using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler<EventCallbackT,
+    ParentHandleT>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -315,6 +315,16 @@ public:
   virtual void
   spin_all(std::chrono::nanoseconds max_duration);
 
+
+  /// Collect work once and execute the next available work, optionally within a duration.
+  /**
+   * This function can be overridden.  The default implementation is suitable for
+   * a single-thread model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * \param[in] timeout The maximum amount of time to spend waiting for work.
+   *   `-1` is potentially block forever waiting for work.
+   */
   RCLCPP_PUBLIC
   virtual void
   spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@@ -413,12 +423,29 @@ public:
   is_spinning();
 
 protected:
+  /// Add a node to executor, execute the next available unit of work, and remove the node.
+  /**
+   * Implementation of spin_node_once using std::chrono::nanoseconds
+   * \param[in] node Shared pointer to the node to add.
+   * \param[in] timeout How long to wait for work to become available. Negative values cause
+   *   spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
+   *   function to be non-blocking.
+   */
   RCLCPP_PUBLIC
   void
   spin_node_once_nanoseconds(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  /// Collect work and execute available work, optionally within a duration.
+  /**
+   * Implementation of spin_some and spin_all.
+   * The exhaustive flag controls if the function will re-collect available work within the duration.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * \param[in] exhaustive when set to true, continue to collect work and execute (spin_all)
+   *                       when set to false, return when all collected work is executed (spin_some)
+   */
   RCLCPP_PUBLIC
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
@@ -433,30 +460,60 @@ protected:
   void
   execute_any_executable(AnyExecutable & any_exec);
 
+  /// Run subscription executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the subscription.
+   * \param[in] subscription Subscription to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  /// Run timer executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the timer callback.
+   * \param[in] timer Timer to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  /// Run service server executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service server callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_service(rclcpp::ServiceBase::SharedPtr service);
 
+  /// Run service client executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service client callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /// Block until more work becomes avilable or timeout is reached.
   /**
+   * Builds a set of waitable entities, which are passed to the middleware.
+   * After building wait set, waits on middleware to notify.
+   * \param[in] timeout duration to wait for new work to become available.
    * \throws std::runtime_error if the wait set can be cleared
    */
   RCLCPP_PUBLIC
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Find node associated with a callback group
+  /**
+   * \param[in] weak_groups_to_nodes map of callback groups to nodes
+   * \param[in] group callback group to find assocatiated node
+   * \return Pointer to associated node if found, else nullptr
+   */
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
@@ -475,6 +532,11 @@ protected:
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
 
+  /// Find the callback group associated with a timer
+  /**
+   * \param[in] timer Timer to find associated callback group
+   * \return Pointer to callback group node if found, else nullptr
+   */
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -502,16 +564,54 @@ protected:
     WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
     bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
 
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * \param[out] any_executable populated union structure of ready executable
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * This is the implementation of get_next_ready_executable that takes into
+   * account the current state of callback groups' association with nodes and
+   * executors.
+   *
+   * This checks in a particular order for available work:
+   *   * Timers
+   *   * Subscriptions
+   *   * Services
+   *   * Clients
+   *   * Waitable
+   *
+   * If the next executable is not associated with this executor/node pair,
+   * then this method will return false.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] weak_groups_to_nodes mapping of callback groups to nodes
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable_from_map(
     AnyExecutable & any_executable,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
+  /// Wait for executable in ready state and populate union structure.
+  /**
+   * If an executable is ready, it will return immediately, otherwise
+   * block based on the timeout for work to become ready.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] timeout duration of time to wait for work, a negative value
+   *   (the defualt behavior), will make this function block indefinitely
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_executable(
@@ -565,10 +665,19 @@ protected:
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
 
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
+
   /// maps nodes to guard conditions
   WeakNodesToGuardConditionsMap
   weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap
   weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);

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/multi_threaded_executor.hpp

@@ -47,7 +47,7 @@ public:
    *
    * \param options common options for all executors
    * \param number_of_threads number of threads to have in the thread pool,
-   *   the default 0 will use the number of cpu cores found instead
+   *   the default 0 will use the number of cpu cores found (minimum of 2)
    * \param yield_before_execute if true std::this_thread::yield() is called
    * \param timeout maximum time to wait
    */

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

@@ -86,8 +86,7 @@ public:
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!has_data_()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+      return BufferT();
     }
 
     auto request = std::move(ring_buffer_[read_index_]);

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

@@ -0,0 +1,70 @@
+// Copyright 2023 iRobot Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_
+
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class provides a wrapper around the waitable object, that is
+ * meant to be used with the EventsExecutor.
+ * The waitset related methods are stubbed out as they should not be called.
+ * This class is abstract as the execute method of rclcpp::Waitable is not implemented.
+ * Nodes who want to implement a custom EventWaitable, can derive from this class and override
+ * the execute method.
+ */
+class EventWaitable : public rclcpp::Waitable
+{
+public:
+  // Constructor
+  RCLCPP_PUBLIC
+  EventWaitable() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  virtual ~EventWaitable() = default;
+
+  // Stub API: not used by EventsExecutor
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) final
+  {
+    (void)wait_set;
+    throw std::runtime_error("EventWaitable can't be checked if it's ready");
+    return false;
+  }
+
+  // Stub API: not used by EventsExecutor
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) final
+  {
+    (void)wait_set;
+    throw std::runtime_error("EventWaitable can't be added to a wait_set");
+  }
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENT_WAITABLE_HPP_

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

@@ -0,0 +1,237 @@
+// 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 <chrono>
+#include <memory>
+#include <queue>
+#include <vector>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_entities_collector.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_notify_waitable.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
+#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
+#include "rclcpp/experimental/timers_manager.hpp"
+#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.
+ * The RMW listener APIs are used to collect new events.
+ *
+ * This executor tries to reduce as much as possible the amount of maintenance operations.
+ * This allows to use customized `EventsQueue` classes to achieve different goals such
+ * 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;
+
+  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:
+  RCLCPP_PUBLIC
+  void
+  spin_once_impl(std::chrono::nanoseconds timeout) override;
+
+  RCLCPP_PUBLIC
+  void
+  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
+
+private:
+  RCLCPP_DISABLE_COPY(EventsExecutor)
+
+  // Execute a single event
+  RCLCPP_PUBLIC
+  void
+  execute_event(const ExecutorEvent & event);
+
+  // Queue where entities can push events
+  rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
+
+  EventsExecutorEntitiesCollector::SharedPtr entities_collector_;
+  EventsExecutorNotifyWaitable::SharedPtr executor_notifier_;
+
+  // Timers manager
+  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_entities_collector.hpp

@@ -0,0 +1,348 @@
+// Copyright 2023 iRobot Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <map>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
+#include "rclcpp/experimental/timers_manager.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+// forward declaration of EventsExecutor to avoid circular dependency
+class EventsExecutor;
+
+typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
+
+/**
+ * @brief This class provides a waitable object that is used for managing the
+ * entities (i.e. nodes and their subscriptions, timers, services, etc)
+ * added to an EventsExecutor.
+ * The add/remove node APIs are used when a node is added/removed from
+ * the associated EventsExecutor and result in setting/unsetting the
+ * events callbacks and adding timers to the timers manager.
+ *
+ * Being this class derived from Waitable, it can be used to wake up an
+ * executor thread while it's spinning.
+ * When this occurs, the execute API takes care of handling changes
+ * in the entities currently used by the executor.
+ */
+class EventsExecutorEntitiesCollector final
+  : public rclcpp::Waitable,
+  public std::enable_shared_from_this<EventsExecutorEntitiesCollector>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(EventsExecutorEntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EventsExecutorEntitiesCollector(
+    EventsExecutor * executor);
+
+  // Destructor
+  RCLCPP_PUBLIC
+  ~EventsExecutorEntitiesCollector() override;
+
+  // Initialize entities collector
+  RCLCPP_PUBLIC
+  void init();
+
+  /// Execute the waitable.
+  RCLCPP_PUBLIC
+  void
+  execute(std::shared_ptr<void> & data) override;
+
+  /// Function to add_handles_to_wait_set and wait for work and
+  /**
+   * block until the wait set is ready or until the timeout has been exceeded.
+   * \throws std::runtime_error if wait set couldn't be cleared or filled.
+   * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
+   */
+  RCLCPP_PUBLIC
+  void
+  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /**
+   * \throws std::runtime_error if it couldn't add guard condition to wait set
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Complete all available queued work without blocking.
+  /**
+   * This function checks if after the guard condition was triggered
+   * (or a spurious wakeup happened) we are really ready to execute
+   * i.e. re-collect entities
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Add a callback group to an executor.
+  /**
+   * \see rclcpp::Executor::add_callback_group
+   * \return boolean whether the node from the callback group is new
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group(
+    rclcpp::CallbackGroup::SharedPtr group_ptr);
+
+  /// Remove a callback group from the executor.
+  /**
+   * \see rclcpp::Executor::remove_callback_group_from_map
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_callback_group_from_map(
+    rclcpp::CallbackGroup::SharedPtr group_ptr,
+    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  /**
+   * \see rclcpp::Executor::add_node()
+   * \throw std::runtime_error if node was already added
+   */
+  RCLCPP_PUBLIC
+  bool
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /**
+   * \see rclcpp::Executor::remove_node()
+   * \throw std::runtime_error if no guard condition is associated with node.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_all_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_manually_added_callback_groups()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_manually_added_callback_groups();
+
+  /// Get callback groups that belong to executor.
+  /**
+   * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
+   */
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::CallbackGroup::WeakPtr>
+  get_automatically_added_callback_groups_from_nodes();
+
+  ///
+  /**
+   * Get the subscription shared pointer corresponding
+   * to a subscription identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(const void * subscription_id);
+
+  ///
+  /**
+   * Get the client shared pointer corresponding
+   * to a client identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(const void * client_id);
+
+  ///
+  /**
+   * Get the service shared pointer corresponding
+   * to a service identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(const void * service_id);
+
+  ///
+  /**
+   * Get the waitable shared pointer corresponding
+   * to a waitable identifier
+   */
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(const void * waitable_id);
+
+  ///
+  /**
+   * Add a weak pointer to a waitable
+   */
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+private:
+  /// Return true if the node belongs to the collector
+  /**
+   * \param[in] node_ptr a node base interface shared pointer
+   * \param[in] weak_groups_to_nodes map to nodes to lookup
+   * \return boolean whether a node belongs the collector
+   */
+  bool
+  has_node(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
+
+  /// Add all callback groups that can be automatically added by any executor
+  /// and is not already associated with an executor from nodes
+  /// that are associated with executor
+  /**
+   * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
+   */
+  void
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  void
+  callback_group_added_impl(
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  node_added_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  void
+  callback_group_removed_impl(
+    rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  node_removed_impl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  void
+  set_entities_event_callbacks_from_map(
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
+
+  void
+  set_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  unset_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
+
+  void
+  set_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
+
+  void
+  unset_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
+
+  std::function<void(size_t)>
+  create_entity_callback(void * exec_entity_id, ExecutorEventType type);
+
+  std::function<void(size_t, int)>
+  create_waitable_callback(void * waitable_id);
+
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
+
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
+    WeakNodesToGuardConditionsMap;
+
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap weak_groups_to_guard_conditions_;
+
+  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
+
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
+  // maps callback groups to nodes.
+  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
+
+  /// List of weak nodes registered in the static executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  // Maps: entity identifiers to weak pointers from the entities registered in the executor
+  // so in the case of an event providing and ID, we can retrieve and own the corresponding
+  // entity while it performs work
+  std::unordered_map<const void *, rclcpp::SubscriptionBase::WeakPtr> weak_subscriptions_map_;
+  std::unordered_map<const void *, rclcpp::ServiceBase::WeakPtr> weak_services_map_;
+  std::unordered_map<const void *, rclcpp::ClientBase::WeakPtr> weak_clients_map_;
+  std::unordered_map<const void *, rclcpp::Waitable::WeakPtr> weak_waitables_map_;
+
+  /// Executor using this entities collector object
+  EventsExecutor * associated_executor_ = nullptr;
+  /// Instance of the timers manager used by the associated executor
+  rclcpp::experimental::TimersManager::SharedPtr timers_manager_;
+
+  // Mutex to protect vector of new nodes.
+  std::recursive_mutex reentrant_mutex_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_ENTITIES_COLLECTOR_HPP_

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

@@ -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 * exec_entity_id;
+  int gen_entity_id;
+  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_executor_notify_waitable.hpp

@@ -0,0 +1,103 @@
+// Copyright 2023 iRobot Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_
+
+#include <list>
+#include <memory>
+
+#include "rcl/guard_condition.h"
+#include "rclcpp/experimental/executors/events_executor/event_waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace executors
+{
+
+/**
+ * @brief This class provides an EventWaitable that allows to
+ * wake up an EventsExecutor when a guard condition is notified.
+ */
+class EventsExecutorNotifyWaitable final : public EventWaitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutorNotifyWaitable)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  EventsExecutorNotifyWaitable() = default;
+
+  // Destructor
+  RCLCPP_PUBLIC
+  virtual ~EventsExecutorNotifyWaitable() = default;
+
+  // The function is a no-op, since we only care of waking up the executor
+  RCLCPP_PUBLIC
+  void
+  execute(std::shared_ptr<void> & data) override
+  {
+    (void)data;
+  }
+
+  RCLCPP_PUBLIC
+  void
+  add_guard_condition(rclcpp::GuardCondition * guard_condition)
+  {
+    notify_guard_conditions_.push_back(guard_condition);
+  }
+
+  RCLCPP_PUBLIC
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    // The second argument of the callback should identify which guard condition
+    // triggered the event. However it's not relevant here as we only
+    // care about waking up the executor, so we pass a 0.
+    auto gc_callback = [callback](size_t count) {
+        callback(count, 0);
+      };
+
+    for (auto gc : notify_guard_conditions_) {
+      gc->set_on_trigger_callback(gc_callback);
+    }
+  }
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data() override
+  {
+    // This waitable doesn't handle any data
+    return nullptr;
+  }
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void) id;
+    return take_data();
+  }
+
+private:
+  std::list<rclcpp::GuardCondition *> notify_guard_conditions_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_NOTIFY_WAITABLE_HPP_

rclcpp/include/rclcpp/experimental/executors/events_executor/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 the insertion/extraction of events in the queue
+  mutable std::mutex mutex_;
+  // Variable used to notify when an event is added to the queue
+  std::condition_variable events_queue_cv_;
+};
+
+}  // namespace executors
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -454,6 +454,8 @@ private:
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
           subscription->provide_intra_process_data(std::move(message));
+          // Last message delivered, break from for loop
+          break;
         } else {
           // Copy the message since we have additional subscriptions to serve
           Deleter deleter = message.get_deleter();
@@ -493,6 +495,8 @@ private:
           if (std::next(it) == subscription_ids.end()) {
             // If this is the last subscription, give up ownership
             ros_message_subscription->provide_intra_process_message(std::move(message));
+            // Last message delivered, break from for loop
+            break;
           } else {
             // Copy the message since we have additional subscriptions to serve
             Deleter deleter = message.get_deleter();

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -109,8 +109,14 @@ public:
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
     } else {
       unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
     }
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
@@ -138,7 +144,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
     }
 
     rmw_message_info_t msg_info;

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -52,7 +52,7 @@ public:
   {}
 
   RCLCPP_PUBLIC
-  virtual ~SubscriptionIntraProcessBase();
+  virtual ~SubscriptionIntraProcessBase() = default;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/experimental/timers_manager.hpp

@@ -0,0 +1,539 @@
+// 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 implementation consists in letting a TimersManager object
+ * to spawn a thread where timers are monitored and periodically executed.
+ * Besides this, other APIs allow to either execute a single timer or all the
+ * currently ready ones.
+ * This class assumes that the `execute_callback()` API of the stored timers is never
+ * called by other entities, but it can only be called from here.
+ * If this assumption is not respected, the heap property may be invalidated,
+ * 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. if not callable,
+   * this object will directly execute timers when they are ready.
+   */
+  RCLCPP_PUBLIC
+  TimersManager(
+    std::shared_ptr<rclcpp::Context> context,
+    std::function<void(void *)> 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 Executes all the timers currently ready when the function was invoked.
+   * This function will lock all the stored timers throughout its duration.
+   * This function is thread safe.
+   * @throws std::runtime_error if the timers thread was already running.
+   */
+  RCLCPP_PUBLIC
+  void execute_ready_timers();
+
+  /**
+   * @brief Get the number of timers that are currently ready.
+   * This function is thread safe.
+   *
+   * @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.
+   *
+   * @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.
+   *
+   * @param timer_id the timer ID of the timer to execute
+   */
+  RCLCPP_PUBLIC
+  void execute_ready_timer(const void * 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;
+    }
+
+    TimerPtr get_timer(const void * 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);
+    }
+
+    void clear_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)
+    {
+      // FIXME!
+      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(void *)> 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_publisher.hpp

@@ -78,38 +78,12 @@ public:
       node_base,
       topic_name,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
-      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos)),
+      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in publisher.hpp.
-    // TODO(nnmm): Deduplicate by moving this into PublisherBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericPublisher() = default;

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -81,45 +81,13 @@ public:
       node_base,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
       topic_name,
-      options.template to_rcl_subscription_options<rclcpp::SerializedMessage>(qos),
+      options.to_rcl_subscription_options(qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
       true),
     callback_(callback),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in subscription.hpp.
-    // TODO(nnmm): Deduplicate by moving this into SubscriptionBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericSubscription() = default;

rclcpp/include/rclcpp/logger.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -122,6 +123,7 @@ private:
   : name_(new std::string(name)) {}
 
   std::shared_ptr<const std::string> name_;
+  std::shared_ptr<std::pair<std::string, std::string>> logger_sublogger_pairname_ = nullptr;
 
 public:
   RCLCPP_PUBLIC
@@ -157,13 +159,7 @@ public:
    */
   RCLCPP_PUBLIC
   Logger
-  get_child(const std::string & suffix)
-  {
-    if (!name_) {
-      return Logger();
-    }
-    return Logger(*name_ + "." + suffix);
-  }
+  get_child(const std::string & suffix);
 
   /// Set level for current logger.
   /**

rclcpp/include/rclcpp/node.hpp

@@ -740,17 +740,21 @@ public:
   /**
    * If the parameter has not been declared, then this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception.
+   * If the parameter has not been initialized, then this method may throw the
+   * rclcpp::exceptions::ParameterUninitializedException exception.
    *
    * If undeclared parameters are allowed, see the node option
    * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will
-   * not throw an exception, and instead return a default initialized
-   * rclcpp::Parameter, which has a type of
+   * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception,
+   * and instead return a default initialized rclcpp::Parameter, which has a type of
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
    *
    * \param[in] name The name of the parameter to get.
    * \return The requested parameter inside of a rclcpp parameter object.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
    *   has not been declared and undeclared parameters are not allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter
+   *   has not been initialized.
    */
   RCLCPP_PUBLIC
   rclcpp::Parameter
@@ -834,12 +838,12 @@ public:
 
   /// Return the parameters by the given parameter names.
   /**
-   * Like get_parameters(), this method may throw the
+   * Like get_parameter(const std::string &), this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception if the
    * requested parameter has not been declared and undeclared parameters are
-   * not allowed.
+   * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception.
    *
-   * Also like get_parameters(), if undeclared parameters are allowed and the
+   * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the
    * parameter has not been declared, then the corresponding rclcpp::Parameter
    * will be default initialized and therefore have the type
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
@@ -849,6 +853,8 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the
    *   parameters have not been declared and undeclared parameters are not
    *   allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if any of the
+   *   parameters have not been initialized.
    */
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>

rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp

@@ -0,0 +1,208 @@
+// Copyright 2022 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__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+
+#include <memory>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// Support and Helper template classes for the NodeInterfaces class.
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n);
+
+/// Stores the interfaces in a tuple, provides constructors, and getters.
+template<typename ... InterfaceTs>
+struct NodeInterfacesStorage
+{
+  template<typename NodeT>
+  NodeInterfacesStorage(NodeT & node)  // NOLINT(runtime/explicit)
+  : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
+  {}
+
+  NodeInterfacesStorage()
+  : interfaces_()
+  {}
+
+  explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
+  : interfaces_(args ...)
+  {}
+
+  /// Individual Node Interface non-const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<NodeInterfaceT>
+  get()
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+  /// Individual Node Interface const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<const NodeInterfaceT>
+  get() const
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+protected:
+  std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
+};
+
+/// Prototype of NodeInterfacesSupports.
+/**
+ * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
+ * if NodeInterfacesSupport is specialized for T, the is_supported should be
+ * set to std::true_type, but by default it is std::false_type, which will
+ * lead to a compiler error when trying to use T with NodeInterfaces.
+ */
+template<typename StorageClassT, typename ... Ts>
+struct NodeInterfacesSupports;
+
+/// Prototype of NodeInterfacesSupportCheck template meta-function.
+/**
+ * This meta-function checks that all the types given are supported,
+ * throwing a more human-readable error if an unsupported type is used.
+ */
+template<typename StorageClassT, typename ... InterfaceTs>
+struct NodeInterfacesSupportCheck;
+
+/// Iterating specialization that ensures classes are supported and inherited.
+template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
+  : public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
+{
+  static_assert(
+    NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
+    "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
+};
+
+/// Terminating case when there are no more "RemainingInterfaceTs".
+template<typename StorageClassT>
+struct NodeInterfacesSupportCheck<StorageClassT>
+{};
+
+/// Default specialization, needs to be specialized for each supported interface.
+template<typename StorageClassT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupports
+{
+  // Specializations need to set this to std::true_type in addition to other interfaces.
+  using is_supported = std::false_type;
+};
+
+/// Terminating specialization of NodeInterfacesSupports.
+template<typename StorageClassT>
+struct NodeInterfacesSupports<StorageClassT>
+  : public StorageClassT
+{
+  /// Perfect forwarding constructor to get arguments down to StorageClassT.
+  template<typename ... ArgsT>
+  explicit NodeInterfacesSupports(ArgsT && ... args)
+  : StorageClassT(std::forward<ArgsT>(args) ...)
+  {}
+};
+
+// Helper functions to initialize the tuple in NodeInterfaces.
+
+template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
+void
+init_element(TupleT & t, NodeT & n)
+{
+  std::get<std::shared_ptr<ElementT>>(t) =
+    NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
+}
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n)
+{
+  using StorageClassT = NodeInterfacesStorage<Ts ...>;
+  std::tuple<std::shared_ptr<Ts>...> t;
+  (init_element<StorageClassT, Ts>(t, n), ...);
+  return t;
+}
+
+/// Macro for creating specializations with less boilerplate.
+/**
+ * You can use this macro to add support for your interface class if:
+ *
+ * - The standard getter is get_node_{NodeInterfaceName}_interface(), and
+ * - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
+ *
+ * Examples of using this can be seen in the standard node interface headers
+ * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
+ *
+ *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+ *
+ * If your interface has a non-standard getter, or you want to instrument it or
+ * something like that, then you'll need to create your own specialization of
+ * the NodeInterfacesSupports struct without this macro.
+ */
+#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
+  namespace rclcpp::node_interfaces::detail { \
+  template<typename StorageClassT, typename ... RemainingInterfaceTs> \
+  struct NodeInterfacesSupports< \
+    StorageClassT, \
+    NodeInterfaceType, \
+    RemainingInterfaceTs ...> \
+    : public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
+  { \
+    using is_supported = std::true_type; \
+ \
+    template<typename NodeT> \
+    static \
+    std::shared_ptr<NodeInterfaceType> \
+    get_from_node_like(NodeT & node_like) \
+    { \
+      return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
+    } \
+ \
+    /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
+    template<typename ... ArgsT> \
+    explicit NodeInterfacesSupports(ArgsT && ... args) \
+      : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+        std::forward<ArgsT>(args) ...) \
+    {} \
+ \
+    std::shared_ptr<NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,7 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
-#include <functional>
+#include <atomic>
 #include <memory>
 #include <mutex>
 #include <string>

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -15,10 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 
+#include <atomic>
+#include <functional>
 #include <memory>
-#include <mutex>
 #include <string>
-#include <vector>
 
 #include "rcl/node.h"
 
@@ -26,6 +26,7 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -177,4 +178,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 
+#include "rcl/time.h"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -42,7 +43,8 @@ public:
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rcl_clock_type_t clock_type);
 
   RCLCPP_PUBLIC
   virtual
@@ -67,7 +69,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
-  rclcpp::Clock::SharedPtr ros_clock_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,4 +51,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 
+#include <atomic>
 #include <chrono>
+#include <condition_variable>
 #include <map>
 #include <memory>
 #include <mutex>

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -29,6 +29,7 @@
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -382,4 +383,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -0,0 +1,164 @@
+// Copyright 2022 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_INTERFACES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+
+#include <memory>
+
+#include "rclcpp/detail/template_unique.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+
+#define ALL_RCLCPP_NODE_INTERFACES \
+  rclcpp::node_interfaces::NodeBaseInterface, \
+  rclcpp::node_interfaces::NodeClockInterface, \
+  rclcpp::node_interfaces::NodeGraphInterface, \
+  rclcpp::node_interfaces::NodeLoggingInterface, \
+  rclcpp::node_interfaces::NodeParametersInterface, \
+  rclcpp::node_interfaces::NodeServicesInterface, \
+  rclcpp::node_interfaces::NodeTimeSourceInterface, \
+  rclcpp::node_interfaces::NodeTimersInterface, \
+  rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeWaitablesInterface
+
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+
+/// A helper class for aggregating node interfaces.
+template<typename ... InterfaceTs>
+class NodeInterfaces
+  : public detail::NodeInterfacesSupportCheck<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >,
+  public detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >
+{
+  static_assert(
+    0 != sizeof ...(InterfaceTs),
+    "must provide at least one interface as a template argument");
+  static_assert(
+    rclcpp::detail::template_unique_v<InterfaceTs ...>,
+    "must provide unique template parameters");
+
+  using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >;
+
+public:
+  /// Create a new NodeInterfaces object using the given node-like object's interfaces.
+  /**
+   * Specify which interfaces you need by passing them as template parameters.
+   *
+   * This allows you to aggregate interfaces from different sources together to pass as a single
+   * aggregate object to any functions that take node interfaces or node-likes, without needing to
+   * templatize that function.
+   *
+   * You may also use this constructor to create a NodeInterfaces that contains a subset of
+   * another NodeInterfaces' interfaces.
+   *
+   * Finally, this class supports implicit conversion from node-like objects, allowing you to
+   * directly pass a node-like to a function that takes a NodeInterfaces object.
+   *
+   * Usage examples:
+   *   ```cpp
+   *   // Suppose we have some function:
+   *   void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
+   *
+   *   // Then we can, explicitly:
+   *   rclcpp::Node node("some_node");
+   *   auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
+   *   fn(ni);
+   *
+   *   // But also:
+   *   fn(node);
+   *
+   *   // Subsetting a NodeInterfaces object also works!
+   *   auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
+   *
+   *   // Or aggregate them (you could aggregate interfaces from disparate node-likes)
+   *   auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
+   *     node->get_node_base_interface(),
+   *     node->get_node_clock_interface()
+   *   )
+   *
+   *   // And then to access the interfaces:
+   *   // Get with get<>
+   *   auto base = ni.get<NodeBaseInterface>();
+   *
+   *   // Or the appropriate getter
+   *   auto clock = ni.get_clock_interface();
+   *   ```
+   *
+   * You may use any of the standard node interfaces that come with rclcpp:
+   *   - rclcpp::node_interfaces::NodeBaseInterface
+   *   - rclcpp::node_interfaces::NodeClockInterface
+   *   - rclcpp::node_interfaces::NodeGraphInterface
+   *   - rclcpp::node_interfaces::NodeLoggingInterface
+   *   - rclcpp::node_interfaces::NodeParametersInterface
+   *   - rclcpp::node_interfaces::NodeServicesInterface
+   *   - rclcpp::node_interfaces::NodeTimeSourceInterface
+   *   - rclcpp::node_interfaces::NodeTimersInterface
+   *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeWaitablesInterface
+   *
+   * Or you use custom interfaces as long as you make a template specialization
+   * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
+   * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
+   *
+   * Usage example:
+   *   ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
+   *
+   * If you choose not to use the helper macro, then you can specialize the
+   * template yourself, but you must:
+   *
+   *   - Provide a template specialization of the get_from_node_like method that gets the interface
+   *     from any node-like that stores the interface, using the node-like's getter
+   *   - Designate the is_supported type as std::true_type using a using directive
+   *   - Provide any number of getter methods to be used to obtain the interface with the
+   *     NodeInterface object, noting that the getters of the storage class will apply to all
+   *     supported interfaces.
+   *     - The getter method names should not clash in name with any other interface getter
+   *       specializations if those other interfaces are meant to be aggregated in the same
+   *       NodeInterfaces object.
+   *
+   * \param[in] node Node-like object from which to get the node interfaces
+   */
+  template<typename NodeT>
+  NodeInterfaces(NodeT & node)  // NOLINT(runtime/explicit)
+  : NodeInterfacesSupportsT(node)
+  {}
+
+  // Create a NodeInterfaces object with no bound interfaces
+  NodeInterfaces()
+  : NodeInterfacesSupportsT()
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_

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/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -58,4 +59,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -15,9 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
+#include <list>
 #include <map>
 #include <memory>
-#include <list>
+#include <mutex>
 #include <string>
 #include <vector>
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,8 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <functional>
 #include <map>
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -276,4 +277,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -62,4 +63,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,4 +38,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -47,4 +48,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -15,8 +15,6 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 
-#include <functional>
-#include <memory>
 #include <string>
 
 #include "rcl/publisher.h"
@@ -24,6 +22,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
@@ -97,4 +96,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "rcl/time.h"
 #include "rcl/node_options.h"
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -46,6 +47,7 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_type = RCL_ROS_TIME
    *   - clock_qos = rclcpp::ClockQoS()
    *   - use_clock_thread = true
    *   - rosout_qos = rclcpp::RosoutQoS()
@@ -246,6 +248,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock type.
+  RCLCPP_PUBLIC
+  const rcl_clock_type_t &
+  clock_type() const;
+
+  /// Set the clock type.
+  /**
+   * The clock type to be used by the node.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_type(const rcl_clock_type_t & clock_type);
+
   /// Return a reference to the clock QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -400,6 +415,8 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rcl_clock_type_t clock_type_ {RCL_ROS_TIME};
+
   rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
 
   bool use_clock_thread_ {true};

rclcpp/include/rclcpp/publisher.hpp

@@ -131,40 +131,16 @@ public:
       node_base,
       topic,
       rclcpp::get_message_type_support_handle<MessageT>(),
-      options.template to_rcl_publisher_options<MessageT>(qos)),
+      options.template to_rcl_publisher_options<MessageT>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     options_(options),
     published_type_allocator_(*options.get_allocator()),
     ros_message_type_allocator_(*options.get_allocator())
   {
     allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_);
     allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
-
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
     // Setup continues in the post construction method, post_init_setup().
   }
 
@@ -405,10 +381,6 @@ public:
     if (!loaned_msg.is_valid()) {
       throw std::runtime_error("loaned message is not valid");
     }
-    if (intra_process_is_enabled_) {
-      // TODO(Karsten1987): support loaned message passed by intraprocess
-      throw std::runtime_error("storing loaned messages in intra process is not supported yet");
-    }
 
     // verify that publisher supports loaned messages
     // TODO(Karsten1987): This case separation has to be done in rclcpp
@@ -422,7 +394,7 @@ public:
     } else {
       // we don't release the ownership, let the middleware copy the ros message
       // and thus the destructor of rclcpp::LoanedMessage cleans up the memory.
-      this->do_inter_process_publish(loaned_msg.get());
+      this->publish(loaned_msg.get());
     }
   }
 

rclcpp/include/rclcpp/publisher_base.hpp

@@ -33,7 +33,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rcpputils/time.hpp"
@@ -78,11 +78,18 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
     const rosidl_message_type_support_t & type_support,
-    const rcl_publisher_options_t & publisher_options);
+    const rcl_publisher_options_t & publisher_options,
+    const PublisherEventCallbacks & event_callbacks,
+    bool use_default_callbacks);
 
   RCLCPP_PUBLIC
   virtual ~PublisherBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this publisher publishes on.
   /** \return The topic name. */
   RCLCPP_PUBLIC
@@ -117,7 +124,7 @@ public:
   /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
   const
-  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get subscription count
@@ -269,7 +276,7 @@ public:
    * If you want more information available in the callback, like the qos event
    * or other information, you may use a lambda with captures or std::bind.
    *
-   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   * \sa rclcpp::EventHandlerBase::set_on_ready_callback
    *
    * \param[in] callback functor to be called when a new event occurs
    * \param[in] event_type identifier for the qos event we want to attach the callback to
@@ -320,7 +327,7 @@ protected:
     const EventCallbackT & callback,
     const rcl_publisher_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+    auto handler = std::make_shared<EventHandler<EventCallbackT,
         std::shared_ptr<rcl_publisher_t>>>(
       callback,
       rcl_publisher_event_init,
@@ -332,12 +339,15 @@ protected:
   RCLCPP_PUBLIC
   void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
 
+  RCLCPP_PUBLIC
+  void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
+
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
   std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
   std::unordered_map<rcl_publisher_event_type_t,
-    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+    std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
     std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
@@ -348,6 +358,8 @@ protected:
   rmw_gid_t rmw_gid_;
 
   const rosidl_message_type_support_t type_support_;
+
+  const PublisherEventCallbacks event_callbacks_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_options.hpp

@@ -26,7 +26,7 @@
 #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
 
 namespace rclcpp

rclcpp/include/rclcpp/qos.hpp

@@ -80,7 +80,9 @@ struct RCLCPP_PUBLIC QoSInitialization
   size_t depth;
 
   /// Constructor which takes both a history policy and a depth (even if it would be unused).
-  QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg);
+  QoSInitialization(
+    rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
+    bool print_depth_warning = true);
 
   /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth.
   static
@@ -97,7 +99,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization
 /// Use to initialize the QoS with the keep_last history setting and the given depth.
 struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
 {
-  explicit KeepLast(size_t depth);
+  explicit KeepLast(size_t depth, bool print_depth_warning = true);
 };
 
 /// Encapsulation of Quality of Service settings.

rclcpp/include/rclcpp/qos_event.hpp

@@ -15,277 +15,8 @@
 #ifndef RCLCPP__QOS_EVENT_HPP_
 #define RCLCPP__QOS_EVENT_HPP_
 
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <stdexcept>
-#include <string>
+#warning This header is obsolete, please include rclcpp/event_handler.hpp instead
 
-#include "rcl/error_handling.h"
-#include "rcl/event_callback.h"
-#include "rmw/impl/cpp/demangle.hpp"
-#include "rmw/incompatible_qos_events_statuses.h"
-
-#include "rcutils/logging_macros.h"
-
-#include "rclcpp/detail/cpp_callback_trampoline.hpp"
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/function_traits.hpp"
-#include "rclcpp/logging.hpp"
-#include "rclcpp/waitable.hpp"
-
-namespace rclcpp
-{
-
-using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
-using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
-using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
-using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
-using QOSMessageLostInfo = rmw_message_lost_status_t;
-using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
-using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
-
-using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
-using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
-using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
-using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
-using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
-using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
-using QOSRequestedIncompatibleQoSCallbackType =
-  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
-
-/// Contains callbacks for various types of events a Publisher can receive from the middleware.
-struct PublisherEventCallbacks
-{
-  QOSDeadlineOfferedCallbackType deadline_callback;
-  QOSLivelinessLostCallbackType liveliness_callback;
-  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
-};
-
-/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
-struct SubscriptionEventCallbacks
-{
-  QOSDeadlineRequestedCallbackType deadline_callback;
-  QOSLivelinessChangedCallbackType liveliness_callback;
-  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
-  QOSMessageLostCallbackType message_lost_callback;
-};
-
-class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
-{
-public:
-  RCLCPP_PUBLIC
-  UnsupportedEventTypeException(
-    rcl_ret_t ret,
-    const rcl_error_state_t * error_state,
-    const std::string & prefix);
-
-  RCLCPP_PUBLIC
-  UnsupportedEventTypeException(
-    const exceptions::RCLErrorBase & base_exc,
-    const std::string & prefix);
-};
-
-class QOSEventHandlerBase : public Waitable
-{
-public:
-  enum class EntityType : std::size_t
-  {
-    Event,
-  };
-
-  RCLCPP_PUBLIC
-  virtual ~QOSEventHandlerBase();
-
-  /// Get the number of ready events
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_ready_events() override;
-
-  /// Add the Waitable to a wait set.
-  RCLCPP_PUBLIC
-  void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
-
-  /// Check if the Waitable is ready.
-  RCLCPP_PUBLIC
-  bool
-  is_ready(rcl_wait_set_t * wait_set) override;
-
-  /// Set a callback to be called when each new event instance occurs.
-  /**
-   * The callback receives a size_t which is the number of events that occurred
-   * since the last time this callback was called.
-   * Normally this is 1, but can be > 1 if events occurred before any
-   * callback was set.
-   *
-   * The callback also receives an int identifier argument.
-   * This is needed because a Waitable may be composed of several distinct entities,
-   * such as subscriptions, services, etc.
-   * The application should provide a generic callback function that will be then
-   * forwarded by the waitable to all of its entities.
-   * Before forwarding, a different value for the identifier argument will be
-   * bond to the function.
-   * This implies that the provided callback can use the identifier to behave
-   * differently depending on which entity triggered the waitable to become ready.
-   *
-   * Since this callback is called from the middleware, you should aim to make
-   * it fast and not blocking.
-   * If you need to do a lot of work or wait for some other event, you should
-   * spin it off to another thread, otherwise you risk blocking the middleware.
-   *
-   * Calling it again will clear any previously set callback.
-   *
-   * An exception will be thrown if the callback is not callable.
-   *
-   * This function is thread-safe.
-   *
-   * If you want more information available in the callback, like the qos event
-   * or other information, you may use a lambda with captures or std::bind.
-   *
-   * \sa rmw_event_set_callback
-   * \sa rcl_event_set_callback
-   *
-   * \param[in] callback functor to be called when a new event occurs
-   */
-  void
-  set_on_ready_callback(std::function<void(size_t, int)> callback) override
-  {
-    if (!callback) {
-      throw std::invalid_argument(
-              "The callback passed to set_on_ready_callback "
-              "is not callable.");
-    }
-
-    // Note: we bind the int identifier argument to this waitable's entity types
-    auto new_callback =
-      [callback, this](size_t number_of_events) {
-        try {
-          callback(number_of_events, static_cast<int>(EntityType::Event));
-        } catch (const std::exception & exception) {
-          RCLCPP_ERROR_STREAM(
-            // TODO(wjwwood): get this class access to the node logger it is associated with
-            rclcpp::get_logger("rclcpp"),
-            "rclcpp::QOSEventHandlerBase@" << this <<
-              " caught " << rmw::impl::cpp::demangle(exception) <<
-              " exception in user-provided callback for the 'on ready' callback: " <<
-              exception.what());
-        } catch (...) {
-          RCLCPP_ERROR_STREAM(
-            rclcpp::get_logger("rclcpp"),
-            "rclcpp::QOSEventHandlerBase@" << this <<
-              " caught unhandled exception in user-provided callback " <<
-              "for the 'on ready' callback");
-        }
-      };
-
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
-
-    // 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.
-    set_on_new_event_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
-      static_cast<const void *>(&new_callback));
-
-    // Store the std::function to keep it in scope, also overwrites the existing one.
-    on_new_event_callback_ = new_callback;
-
-    // Set it again, now using the permanent storage.
-    set_on_new_event_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
-      static_cast<const void *>(&on_new_event_callback_));
-  }
-
-  /// Unset the callback registered for new events, if any.
-  void
-  clear_on_ready_callback() override
-  {
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
-    if (on_new_event_callback_) {
-      set_on_new_event_callback(nullptr, nullptr);
-      on_new_event_callback_ = nullptr;
-    }
-  }
-
-protected:
-  RCLCPP_PUBLIC
-  void
-  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
-
-  rcl_event_t event_handle_;
-  size_t wait_set_event_index_;
-  std::recursive_mutex callback_mutex_;
-  std::function<void(size_t)> on_new_event_callback_{nullptr};
-};
-
-template<typename EventCallbackT, typename ParentHandleT>
-class QOSEventHandler : public QOSEventHandlerBase
-{
-public:
-  template<typename InitFuncT, typename EventTypeEnum>
-  QOSEventHandler(
-    const EventCallbackT & callback,
-    InitFuncT init_func,
-    ParentHandleT parent_handle,
-    EventTypeEnum event_type)
-  : parent_handle_(parent_handle), event_callback_(callback)
-  {
-    event_handle_ = rcl_get_zero_initialized_event();
-    rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
-    if (ret != RCL_RET_OK) {
-      if (ret == RCL_RET_UNSUPPORTED) {
-        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
-        rcl_reset_error();
-        throw exc;
-      } else {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
-      }
-    }
-  }
-
-  /// Take data so that the callback cannot be scheduled again
-  std::shared_ptr<void>
-  take_data() override
-  {
-    EventCallbackInfoT callback_info;
-    rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
-    if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Couldn't take event info: %s", rcl_get_error_string().str);
-      return nullptr;
-    }
-    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
-  }
-
-  std::shared_ptr<void>
-  take_data_by_entity_id(size_t id) override
-  {
-    (void)id;
-    return take_data();
-  }
-
-  /// Execute any entities of the Waitable that are ready.
-  void
-  execute(std::shared_ptr<void> & data) override
-  {
-    if (!data) {
-      throw std::runtime_error("'data' is empty");
-    }
-    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
-    event_callback_(*callback_ptr);
-    callback_ptr.reset();
-  }
-
-private:
-  using EventCallbackInfoT = typename std::remove_reference<typename
-      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
-
-  ParentHandleT parent_handle_;
-  EventCallbackT event_callback_;
-};
-
-}  // namespace rclcpp
+#include "rclcpp/event_handler.hpp"
 
 #endif  // RCLCPP__QOS_EVENT_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -26,6 +26,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
 #include "rcl/service.h"
+#include "rcl/service_introspection.h"
 
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -34,6 +35,7 @@
 #include "tracetools/tracetools.h"
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -55,7 +57,7 @@ public:
   explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
-  virtual ~ServiceBase();
+  virtual ~ServiceBase() = default;
 
   /// Return the name of the service.
   /** \return The name of the service. */
@@ -222,7 +224,7 @@ public:
     // 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.
     set_on_new_request_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -230,7 +232,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_request_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_request_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_request_callback_));
   }
 
@@ -307,11 +310,9 @@ public:
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
     rcl_service_options_t & service_options)
-  : ServiceBase(node_handle), any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
-
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
       new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
@@ -330,7 +331,7 @@ public:
     rcl_ret_t ret = rcl_service_init(
       service_handle_.get(),
       node_handle.get(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &service_options);
     if (ret != RCL_RET_OK) {
@@ -370,8 +371,8 @@ public:
     std::shared_ptr<rcl_node_t> node_handle,
     std::shared_ptr<rcl_service_t> service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle.get())) {
@@ -405,8 +406,8 @@ public:
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle)) {
@@ -486,10 +487,39 @@ public:
     }
   }
 
+  /// Configure client 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
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_service_configure_service_introspection(
+      service_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection");
+    }
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Service)
 
   AnyServiceCallback<ServiceT> any_callback_;
+
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <vector>
+#include <utility>
 
 #include "rcl/allocator.h"
 
@@ -120,8 +121,8 @@ public:
       }
     }
     for (size_t i = 0; i < waitable_handles_.size(); ++i) {
-      if (!waitable_handles_[i]->is_ready(wait_set)) {
-        waitable_handles_[i].reset();
+      if (waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i]));
       }
     }
 
@@ -145,10 +146,7 @@ public:
       timer_handles_.end()
     );
 
-    waitable_handles_.erase(
-      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
-      waitable_handles_.end()
-    );
+    waitable_handles_.clear();
   }
 
   bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
@@ -392,8 +390,9 @@ public:
     rclcpp::AnyExecutable & any_exec,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
-    auto it = waitable_handles_.begin();
-    while (it != waitable_handles_.end()) {
+    auto & waitable_handles = waitable_triggered_handles_;
+    auto it = waitable_handles.begin();
+    while (it != waitable_handles.end()) {
       std::shared_ptr<Waitable> & waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
@@ -401,7 +400,7 @@ public:
         if (!group) {
           // Group was not found, meaning the waitable is not valid...
           // Remove it from the ready list and continue looking
-          it = waitable_handles_.erase(it);
+          it = waitable_handles.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -414,11 +413,11 @@ public:
         any_exec.waitable = waitable;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
-        waitable_handles_.erase(it);
+        waitable_handles.erase(it);
         return;
       }
       // Else, the waitable is no longer valid, remove it and continue
-      it = waitable_handles_.erase(it);
+      it = waitable_handles.erase(it);
     }
   }
 
@@ -499,6 +498,8 @@ private:
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
   VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
+  VectorRebind<std::shared_ptr<Waitable>> waitable_triggered_handles_;
+
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

rclcpp/include/rclcpp/subscription.hpp

@@ -140,44 +140,15 @@ public:
       node_base,
       type_support_handle,
       topic_name,
-      options.template to_rcl_subscription_options<ROSMessageType>(qos),
+      options.to_rcl_subscription_options(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks,
       callback.is_serialized_message_callback()),
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
   {
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options_.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;

rclcpp/include/rclcpp/subscription_base.hpp

@@ -37,7 +37,7 @@
 #include "rclcpp/message_info.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/serialized_message.hpp"
 #include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
@@ -84,12 +84,20 @@ public:
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
+    const SubscriptionEventCallbacks & event_callbacks,
+    bool use_default_callbacks,
     bool is_serialized = false);
 
   /// Destructor.
   RCLCPP_PUBLIC
   virtual ~SubscriptionBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(
+    const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this subscription is subscribed on.
   RCLCPP_PUBLIC
   const char *
@@ -107,7 +115,7 @@ public:
   /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
   const
-  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get the actual QoS settings, after the defaults have been determined.
@@ -348,7 +356,7 @@ public:
     // 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.
     set_on_new_message_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -356,7 +364,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_message_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_message_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_message_callback_));
   }
 
@@ -448,7 +457,7 @@ public:
    * If you want more information available in the callback, like the qos event
    * or other information, you may use a lambda with captures or std::bind.
    *
-   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   * \sa rclcpp::EventHandlerBase::set_on_ready_callback
    *
    * \param[in] callback functor to be called when a new event occurs
    * \param[in] event_type identifier for the qos event we want to attach the callback to
@@ -533,7 +542,7 @@ protected:
     const EventCallbackT & callback,
     const rcl_subscription_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+    auto handler = std::make_shared<EventHandler<EventCallbackT,
         std::shared_ptr<rcl_subscription_t>>>(
       callback,
       rcl_subscription_event_init,
@@ -546,6 +555,9 @@ protected:
   RCLCPP_PUBLIC
   void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const;
 
+  RCLCPP_PUBLIC
+  void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
+
   RCLCPP_PUBLIC
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
@@ -562,13 +574,15 @@ protected:
   rclcpp::Logger node_logger_;
 
   std::unordered_map<rcl_subscription_event_type_t,
-    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+    std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_subscription_id_;
   std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
 
+  const SubscriptionEventCallbacks event_callbacks_;
+
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
@@ -577,7 +591,7 @@ private:
 
   std::atomic<bool> subscription_in_use_by_wait_set_{false};
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
-  std::unordered_map<rclcpp::QOSEventHandlerBase *,
+  std::unordered_map<rclcpp::EventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
 
   std::recursive_mutex callback_mutex_;

rclcpp/include/rclcpp/subscription_options.hpp

@@ -26,7 +26,7 @@
 #include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
 #include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
@@ -110,7 +110,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
    * \param qos QoS profile for subcription.
    * \return rcl_subscription_options_t structure based on the rclcpp::QoS
    */
-  template<typename MessageT>
   rcl_subscription_options_t
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {

rclcpp/include/rclcpp/timer.hpp

@@ -149,11 +149,48 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Set a callback to be called when the timer is reset
+  /**
+   * You should aim to make this callback fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread.
+   *
+   * Calling it again will override any previously set callback.
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback,
+   * you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called whenever timer is reset
+   */
+  RCLCPP_PUBLIC
+  void
+  set_on_reset_callback(std::function<void(size_t)> callback);
+
+  /// Unset the callback registered for reset timer
+  RCLCPP_PUBLIC
+  void
+  clear_on_reset_callback();
+
 protected:
+  std::recursive_mutex callback_mutex_;
+  // Declare callback before timer_handle_, so on destruction
+  // the callback is destroyed last. Otherwise, the rcl timer
+  // callback would point briefly to a destroyed function.
+  // Clearing the callback on timer destructor also makes sure
+  // the rcl callback is cleared before on_reset_callback_.
+  std::function<void(size_t)> on_reset_callback_{nullptr};
+
   Clock::SharedPtr clock_;
   std::shared_ptr<rcl_timer_t> timer_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  RCLCPP_PUBLIC
+  void
+  set_on_reset_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
 
@@ -190,10 +227,16 @@ public:
       rclcpp_timer_callback_added,
       static_cast<const void *>(get_timer_handle().get()),
       reinterpret_cast<const void *>(&callback_));
-    TRACEPOINT(
-      rclcpp_callback_register,
-      reinterpret_cast<const void *>(&callback_),
-      tracetools::get_symbol(callback_));
+#ifndef TRACETOOLS_DISABLED
+    if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+      char * symbol = tracetools::get_symbol(callback_);
+      DO_TRACEPOINT(
+        rclcpp_callback_register,
+        reinterpret_cast<const void *>(&callback_),
+        symbol);
+      std::free(symbol);
+    }
+#endif
   }
 
   /// Default destructor.

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -516,7 +516,7 @@ public:
    * the waitable to be removed, but it will cause the associated entity pointer
    * to be nullptr when introspecting this waitable after waiting.
    *
-   * Note that rclcpp::QOSEventHandlerBase are just a special case of
+   * Note that rclcpp::EventHandlerBase is just a special case of
    * rclcpp::Waitable and can be added with this function.
    *
    * \param[in] waitable Waitable to be added.

rclcpp/package.xml

@@ -2,13 +2,17 @@
 <?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>17.0.0</version>
+  <version>19.3.0</version>
   <description>The ROS client library in C++.</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>

rclcpp/resource/logging.hpp.em

@@ -124,7 +124,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
+      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \

rclcpp/src/rclcpp/callback_group.cpp

@@ -12,9 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/callback_group.hpp"
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
 
-#include <vector>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/waitable.hpp"
 
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
@@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup(
   automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
+CallbackGroup::~CallbackGroup()
+{
+  trigger_notify_guard_condition();
+}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -97,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
+rclcpp::GuardCondition::SharedPtr
+CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  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_;
+}
+
+void
+CallbackGroup::trigger_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_) {
+    notify_guard_condition_->trigger();
+  }
+}
+
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/client.cpp

@@ -23,9 +23,11 @@
 #include "rcl/graph.h"
 #include "rcl/node.h"
 #include "rcl/wait.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/logging.hpp"
 
@@ -65,13 +67,6 @@ ClientBase::ClientBase(
     });
 }
 
-ClientBase::~ClientBase()
-{
-  clear_on_new_response_callback();
-  // Make sure the client handle is destructed as early as possible and before the node handle
-  client_handle_.reset();
-}
-
 bool
 ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out)
 {
@@ -248,7 +243,6 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo
     user_data);
 
   if (RCL_RET_OK != ret) {
-    using rclcpp::exceptions::throw_from_rcl_error;
     throw_from_rcl_error(ret, "failed to set the on new response callback for client");
   }
 }

rclcpp/src/rclcpp/clock.cpp

@@ -67,7 +67,7 @@ Clock::Clock(rcl_clock_type_t clock_type)
 Clock::~Clock() {}
 
 Time
-Clock::now()
+Clock::now() const
 {
   Time now(0, 0, impl_->rcl_clock_.type);
 
@@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
   return sleep_until(now() + rel_time, context);
 }
 
+bool
+Clock::started()
+{
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock is not rcl_clock_valid");
+  }
+  return rcl_clock_time_started(get_clock_handle());
+}
+
+bool
+Clock::wait_until_started(Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  if (started()) {
+    return true;
+  } else {
+    // Wait until the first non-zero time
+    return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
+  }
+}
+
+bool
+Clock::wait_until_started(
+  const Duration & timeout,
+  Context::SharedPtr context,
+  const Duration & wait_tick_ns)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  Clock timeout_clock = Clock(RCL_STEADY_TIME);
+  Time start = timeout_clock.now();
+
+  // Check if the clock has started every wait_tick_ns nanoseconds
+  // Context check checks for rclcpp::shutdown()
+  while (!started() && context->is_valid()) {
+    if (timeout < wait_tick_ns) {
+      timeout_clock.sleep_for(timeout);
+    } else {
+      Duration time_left = start + timeout - timeout_clock.now();
+      if (time_left > wait_tick_ns) {
+        timeout_clock.sleep_for(Duration(wait_tick_ns));
+      } else {
+        timeout_clock.sleep_for(time_left);
+      }
+    }
+
+    if (timeout_clock.now() - start > timeout) {
+      return started();
+    }
+  }
+  return started();
+}
+
+
 bool
 Clock::ros_time_is_active()
 {

rclcpp/src/rclcpp/context.cpp

@@ -219,7 +219,7 @@ Context::init(
     if (0u == count) {
       ret = rcl_logging_configure_with_output_handler(
         &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
+        rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
         rclcpp_logging_output_handler);
       if (RCL_RET_OK != ret) {
         rcl_context_.reset();
@@ -365,49 +365,45 @@ Context::on_shutdown(OnShutdownCallback callback)
 rclcpp::OnShutdownCallbackHandle
 Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  return add_shutdown_callback(ShutdownType::on_shutdown, callback);
+  return add_shutdown_callback<ShutdownType::on_shutdown>(callback);
 }
 
 bool
 Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle);
+  return remove_shutdown_callback<ShutdownType::on_shutdown>(callback_handle);
 }
 
 rclcpp::PreShutdownCallbackHandle
 Context::add_pre_shutdown_callback(PreShutdownCallback callback)
 {
-  return add_shutdown_callback(ShutdownType::pre_shutdown, callback);
+  return add_shutdown_callback<ShutdownType::pre_shutdown>(callback);
 }
 
 bool
 Context::remove_pre_shutdown_callback(
   const PreShutdownCallbackHandle & callback_handle)
 {
-  return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
+  return remove_shutdown_callback<ShutdownType::pre_shutdown>(callback_handle);
 }
 
+template<Context::ShutdownType shutdown_type>
 rclcpp::ShutdownCallbackHandle
 Context::add_shutdown_callback(
-  ShutdownType shutdown_type,
   ShutdownCallback callback)
 {
   auto callback_shared_ptr =
     std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);
 
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      {
-        std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
-        pre_shutdown_callbacks_.emplace(callback_shared_ptr);
-      }
-      break;
-    case ShutdownType::on_shutdown:
-      {
-        std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-        on_shutdown_callbacks_.emplace(callback_shared_ptr);
-      }
-      break;
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
+    pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);
+  } else {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    on_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   }
 
   ShutdownCallbackHandle callback_handle;
@@ -415,73 +411,74 @@ Context::add_shutdown_callback(
   return callback_handle;
 }
 
+template<Context::ShutdownType shutdown_type>
 bool
 Context::remove_shutdown_callback(
-  ShutdownType shutdown_type,
   const ShutdownCallbackHandle & callback_handle)
 {
-  std::mutex * mutex_ptr = nullptr;
-  std::unordered_set<
-    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
-
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      mutex_ptr = &pre_shutdown_callbacks_mutex_;
-      callback_list_ptr = &pre_shutdown_callbacks_;
-      break;
-    case ShutdownType::on_shutdown:
-      mutex_ptr = &on_shutdown_callbacks_mutex_;
-      callback_list_ptr = &on_shutdown_callbacks_;
-      break;
-  }
-
-  std::lock_guard<std::mutex> lock(*mutex_ptr);
-  auto callback_shared_ptr = callback_handle.callback.lock();
+  const auto callback_shared_ptr = callback_handle.callback.lock();
   if (callback_shared_ptr == nullptr) {
     return false;
   }
-  return callback_list_ptr->erase(callback_shared_ptr) == 1;
+
+  const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {
+      const std::lock_guard<std::mutex> lock(mutex);
+      auto iter = callback_vector.begin();
+      for (; iter != callback_vector.end(); iter++) {
+        if ((*iter).get() == callback_shared_ptr.get()) {
+          break;
+        }
+      }
+      if (iter == callback_vector.end()) {
+        return false;
+      }
+      callback_vector.erase(iter);
+      return true;
+    };
+
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+  } else {
+    return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
+  }
 }
 
 std::vector<rclcpp::Context::OnShutdownCallback>
 Context::get_on_shutdown_callbacks() const
 {
-  return get_shutdown_callback(ShutdownType::on_shutdown);
+  return get_shutdown_callback<ShutdownType::on_shutdown>();
 }
 
 std::vector<rclcpp::Context::PreShutdownCallback>
 Context::get_pre_shutdown_callbacks() const
 {
-  return get_shutdown_callback(ShutdownType::pre_shutdown);
+  return get_shutdown_callback<ShutdownType::pre_shutdown>();
 }
 
+template<Context::ShutdownType shutdown_type>
 std::vector<rclcpp::Context::ShutdownCallback>
-Context::get_shutdown_callback(ShutdownType shutdown_type) const
+Context::get_shutdown_callback() const
 {
-  std::mutex * mutex_ptr = nullptr;
-  const std::unordered_set<
-    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+  const auto get_callback_vector = [this](auto & mutex, auto & callback_set) {
+      const std::lock_guard<std::mutex> lock(mutex);
+      std::vector<rclcpp::Context::ShutdownCallback> callbacks;
+      for (auto & callback : callback_set) {
+        callbacks.push_back(*callback);
+      }
+      return callbacks;
+    };
 
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      mutex_ptr = &pre_shutdown_callbacks_mutex_;
-      callback_list_ptr = &pre_shutdown_callbacks_;
-      break;
-    case ShutdownType::on_shutdown:
-      mutex_ptr = &on_shutdown_callbacks_mutex_;
-      callback_list_ptr = &on_shutdown_callbacks_;
-      break;
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+  } else {
+    return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
   }
-
-  std::vector<rclcpp::Context::ShutdownCallback> callbacks;
-  {
-    std::lock_guard<std::mutex> lock(*mutex_ptr);
-    for (auto & iter : *callback_list_ptr) {
-      callbacks.emplace_back(*iter);
-    }
-  }
-
-  return callbacks;
 }
 
 std::shared_ptr<rcl_context_t>

rclcpp/src/rclcpp/event_handler.cpp

@@ -12,9 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <stdexcept>
 #include <string>
 
-#include "rclcpp/qos_event.hpp"
+#include "rcl/event.h"
+
+#include "rclcpp/event_handler.hpp"
+#include "rclcpp/exceptions/exceptions.hpp"
 
 namespace rclcpp
 {
@@ -33,11 +37,14 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
   std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message)
 {}
 
-QOSEventHandlerBase::~QOSEventHandlerBase()
+EventHandlerBase::~EventHandlerBase()
 {
-  if (on_new_event_callback_) {
-    clear_on_ready_callback();
-  }
+  // 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(
@@ -49,14 +56,14 @@ QOSEventHandlerBase::~QOSEventHandlerBase()
 
 /// Get the number of ready events.
 size_t
-QOSEventHandlerBase::get_number_of_ready_events()
+EventHandlerBase::get_number_of_ready_events()
 {
   return 1;
 }
 
 /// Add the Waitable to a wait set.
 void
-QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_);
   if (RCL_RET_OK != ret) {
@@ -66,13 +73,13 @@ QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 /// Check if the Waitable is ready.
 bool
-QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
+EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
 {
   return wait_set->events[wait_set_event_index_] == &event_handle_;
 }
 
 void
-QOSEventHandlerBase::set_on_new_event_callback(
+EventHandlerBase::set_on_new_event_callback(
   rcl_event_callback_t callback,
   const void * user_data)
 {
@@ -83,7 +90,7 @@ QOSEventHandlerBase::set_on_new_event_callback(
 
   if (RCL_RET_OK != ret) {
     using rclcpp::exceptions::throw_from_rcl_error;
-    throw_from_rcl_error(ret, "failed to set the on new message callback for QOS Event");
+    throw_from_rcl_error(ret, "failed to set the on new message callback for Event");
   }
 }
 

rclcpp/src/rclcpp/executor.cpp

@@ -106,6 +106,12 @@ Executor::~Executor()
   weak_groups_associated_with_executor_to_nodes_.clear();
   weak_groups_to_nodes_associated_with_executor_.clear();
   weak_groups_to_nodes_.clear();
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
+    auto guard_condition = pair.second;
+    memory_strategy_->remove_guard_condition(guard_condition);
+  }
+  weak_groups_to_guard_conditions_.clear();
+
   for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
@@ -204,8 +210,7 @@ Executor::add_callback_group_to_map(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Callback group has already been added to an executor.");
   }
-  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info =
     weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -215,21 +220,24 @@ Executor::add_callback_group_to_map(
   }
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-  if (is_new_node) {
-    const auto & gc = node_ptr->get_notify_guard_condition();
-    weak_nodes_to_guard_conditions_[node_ptr] = &gc;
-    if (notify) {
-      // Interrupt waiting to handle new node
-      try {
-        interrupt_guard_condition_.trigger();
-      } catch (const rclcpp::exceptions::RCLError & ex) {
-        throw std::runtime_error(
-                std::string(
-                  "Failed to trigger guard condition on callback group add: ") + ex.what());
-      }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+    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);
+  }
+
+  if (notify) {
+    // Interrupt waiting to handle new node
+    try {
+      interrupt_guard_condition_.trigger();
+    } catch (const rclcpp::exceptions::RCLError & ex) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to trigger guard condition on callback group add: ") + ex.what());
     }
-    // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(gc);
   }
 }
 
@@ -272,6 +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;
+  // Add the node's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(gc);
   weak_nodes_.push_back(node_ptr);
 }
 
@@ -300,7 +312,12 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    weak_nodes_to_guard_conditions_.erase(node_ptr);
+    auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (iter != weak_groups_to_guard_conditions_.end()) {
+      memory_strategy_->remove_guard_condition(iter->second);
+    }
+    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
     if (notify) {
       try {
         interrupt_guard_condition_.trigger();
@@ -310,7 +327,6 @@ Executor::remove_callback_group_from_map(
                   "Failed to trigger guard condition on callback group remove: ") + ex.what());
       }
     }
-    memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -372,6 +388,9 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     }
   }
 
+  memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
+  weak_nodes_to_guard_conditions_.erase(node_ptr);
+
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   has_executor.store(false);
 }
@@ -721,6 +740,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
           {
             weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
           }
+          auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr);
+          if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) {
+            auto guard_condition = callback_guard_pair->second;
+            weak_groups_to_guard_conditions_.erase(group_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
           weak_groups_to_nodes_.erase(group_ptr);
         });
     }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -21,6 +21,7 @@
 
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/logging.hpp"
 #include "rclcpp/utilities.hpp"
 
 using rclcpp::executors::MultiThreadedExecutor;
@@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor(
   yield_before_execute_(yield_before_execute),
   next_exec_timeout_(next_exec_timeout)
 {
-  number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
-  if (number_of_threads_ == 0) {
-    number_of_threads_ = 1;
+  number_of_threads_ = number_of_threads > 0 ?
+    number_of_threads :
+    std::max(std::thread::hardware_concurrency(), 2U);
+
+  if (number_of_threads_ == 1) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "MultiThreadedExecutor is used with a single thread.\n"
+      "Use the SingleThreadedExecutor instead.");
   }
 }
 

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

@@ -0,0 +1,327 @@
+// 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 <queue>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcpputils/scope_exit.hpp"
+
+#include "rclcpp/exceptions/exceptions.hpp"
+
+using namespace std::chrono_literals;
+
+using rclcpp::experimental::executors::EventsExecutor;
+
+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
+  std::function<void(void *)> timer_on_ready_cb = nullptr;
+  if (execute_timers_separate_thread) {
+    timer_on_ready_cb = [this](const void * timer_id) {
+        ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
+        this->events_queue_->enqueue(event);
+      };
+  }
+  timers_manager_ =
+    std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
+
+  // Create entities collector
+  entities_collector_ = std::make_shared<EventsExecutorEntitiesCollector>(this);
+  entities_collector_->init();
+
+  // Setup the executor notifier to wake up the executor when some guard conditions are tiggered.
+  // The added guard conditions are guaranteed to not go out of scope before the executor itself.
+  executor_notifier_ = std::make_shared<EventsExecutorNotifyWaitable>();
+  executor_notifier_->add_guard_condition(shutdown_guard_condition_.get());
+  executor_notifier_->add_guard_condition(&interrupt_guard_condition_);
+
+  entities_collector_->add_waitable(executor_notifier_);
+}
+
+EventsExecutor::~EventsExecutor()
+{
+  spinning.store(false);
+}
+
+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
+  entities_collector_->add_node(node_ptr);
+}
+
+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.
+  entities_collector_->remove_node(node_ptr);
+}
+
+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 = entities_collector_->get_client(event.exec_entity_id);
+
+        if (client) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_client(client);
+          }
+        }
+        break;
+      }
+    case ExecutorEventType::SUBSCRIPTION_EVENT:
+      {
+        auto subscription = entities_collector_->get_subscription(event.exec_entity_id);
+
+        if (subscription) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_subscription(subscription);
+          }
+        }
+        break;
+      }
+    case ExecutorEventType::SERVICE_EVENT:
+      {
+        auto service = entities_collector_->get_service(event.exec_entity_id);
+
+        if (service) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            execute_service(service);
+          }
+        }
+        break;
+      }
+    case ExecutorEventType::TIMER_EVENT:
+      {
+        timers_manager_->execute_ready_timer(event.exec_entity_id);
+        break;
+      }
+    case ExecutorEventType::WAITABLE_EVENT:
+      {
+        auto waitable = entities_collector_->get_waitable(event.exec_entity_id);
+
+        if (waitable) {
+          for (size_t i = 0; i < event.num_events; i++) {
+            auto data = waitable->take_data_by_entity_id(event.gen_entity_id);
+            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;
+  entities_collector_->add_callback_group(group_ptr, node_ptr);
+}
+
+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;
+  entities_collector_->remove_callback_group(group_ptr);
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutor::get_all_callback_groups()
+{
+  return entities_collector_->get_all_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutor::get_manually_added_callback_groups()
+{
+  return entities_collector_->get_manually_added_callback_groups();
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutor::get_automatically_added_callback_groups_from_nodes()
+{
+  return entities_collector_->get_automatically_added_callback_groups_from_nodes();
+}

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

@@ -0,0 +1,699 @@
+// Copyright 2023 iRobot Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/experimental/executors/events_executor/events_executor_entities_collector.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
+#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
+
+using rclcpp::experimental::executors::ExecutorEvent;
+using rclcpp::experimental::executors::ExecutorEventType;
+using rclcpp::experimental::executors::EventsExecutorEntitiesCollector;
+
+EventsExecutorEntitiesCollector::EventsExecutorEntitiesCollector(
+  rclcpp::experimental::executors::EventsExecutor * executor)
+{
+  if (executor == nullptr) {
+    throw std::runtime_error("Received nullptr executor in EventsExecutorEntitiesCollector.");
+  }
+
+  associated_executor_ = executor;
+  timers_manager_ = associated_executor_->timers_manager_;
+}
+
+void
+EventsExecutorEntitiesCollector::init()
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // Add the EventsExecutorEntitiesCollector shared_ptr to waitables map
+  weak_waitables_map_.emplace(this, this->shared_from_this());
+}
+
+EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector()
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  // Disassociate all callback groups and thus nodes.
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+      callback_group_removed_impl(group);
+    }
+  }
+  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    auto group = pair.first.lock();
+    if (group) {
+      std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
+      has_executor.store(false);
+      callback_group_removed_impl(group);
+    }
+  }
+  // Disassociate all nodes
+  for (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
+      has_executor.store(false);
+      node_removed_impl(node);
+    }
+  }
+
+  // Unset callback group notify guard condition executor callback
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
+    auto group = pair.first.lock();
+    if (group) {
+      auto & group_gc = pair.second;
+      unset_guard_condition_callback(group_gc);
+    }
+  }
+
+  weak_clients_map_.clear();
+  weak_services_map_.clear();
+  weak_waitables_map_.clear();
+  weak_subscriptions_map_.clear();
+  weak_nodes_to_guard_conditions_.clear();
+  weak_groups_to_guard_conditions_.clear();
+
+  weak_groups_associated_with_executor_to_nodes_.clear();
+  weak_groups_to_nodes_associated_with_executor_.clear();
+  weak_nodes_.clear();
+}
+
+void
+EventsExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
+{
+  // This function is called when the associated executor is notified that something changed.
+  // We do not know if an entity has been added or removed so we have to rebuild everything.
+  (void)data;
+
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  timers_manager_->clear();
+
+  // If a registered node has a new callback group, register the group.
+  add_callback_groups_from_nodes_associated_to_executor();
+
+  // For all groups registered in the executor, set their event callbacks.
+  set_entities_event_callbacks_from_map(weak_groups_associated_with_executor_to_nodes_);
+  set_entities_event_callbacks_from_map(weak_groups_to_nodes_associated_with_executor_);
+}
+
+std::shared_ptr<void>
+EventsExecutorEntitiesCollector::take_data()
+{
+  return nullptr;
+}
+
+std::shared_ptr<void>
+EventsExecutorEntitiesCollector::take_data_by_entity_id(size_t id)
+{
+  (void)id;
+  return nullptr;
+}
+
+void
+EventsExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  (void)wait_set;
+}
+
+bool
+EventsExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
+{
+  (void)p_wait_set;
+  return false;
+}
+
+bool
+EventsExecutorEntitiesCollector::add_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  bool is_new_node = false;
+  // If the node already has an executor
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Node has already been added to an executor.");
+  }
+  node_ptr->for_each_callback_group(
+    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      if (
+        !group_ptr->get_associated_with_executor_atomic().load() &&
+        group_ptr->automatically_add_to_executor_with_node())
+      {
+        is_new_node = (add_callback_group(
+          group_ptr,
+          node_ptr,
+          weak_groups_to_nodes_associated_with_executor_) ||
+        is_new_node);
+      }
+    });
+  weak_nodes_.push_back(node_ptr);
+  return is_new_node;
+}
+
+bool
+EventsExecutorEntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  // If the callback_group already has an executor
+  std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
+  if (has_executor.exchange(true)) {
+    throw std::runtime_error("Callback group has already been added to an executor.");
+  }
+  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto insert_info = weak_groups_to_nodes.insert(
+    std::make_pair(weak_group_ptr, node_ptr));
+  bool was_inserted = insert_info.second;
+  if (!was_inserted) {
+    throw std::runtime_error("Callback group was already added to executor.");
+  }
+
+  if (is_new_node) {
+    node_added_impl(node_ptr);
+  }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+
+    rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
+  }
+
+  callback_group_added_impl(group_ptr);
+
+  return is_new_node;
+}
+
+bool
+EventsExecutorEntitiesCollector::add_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+EventsExecutorEntitiesCollector::remove_callback_group(
+  rclcpp::CallbackGroup::SharedPtr group_ptr)
+{
+  return this->remove_callback_group_from_map(
+    group_ptr,
+    weak_groups_associated_with_executor_to_nodes_);
+}
+
+bool
+EventsExecutorEntitiesCollector::remove_callback_group_from_map(
+  rclcpp::CallbackGroup::SharedPtr group_ptr,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
+  auto iter = weak_groups_to_nodes.find(weak_group_ptr);
+  if (iter != weak_groups_to_nodes.end()) {
+    node_ptr = iter->second.lock();
+    if (node_ptr == nullptr) {
+      throw std::runtime_error("Node must not be deleted before its callback group(s).");
+    }
+    weak_groups_to_nodes.erase(iter);
+    callback_group_removed_impl(group_ptr);
+  } else {
+    throw std::runtime_error("Callback group needs to be associated with executor.");
+  }
+  // If the node was matched and removed, interrupt waiting.
+  bool node_removed = false;
+  if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
+  {
+    node_removed_impl(node_ptr);
+    node_removed = true;
+  }
+
+  weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
+  return node_removed;
+}
+
+bool
+EventsExecutorEntitiesCollector::remove_node(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
+{
+  if (!node_ptr->get_associated_with_executor_atomic().load()) {
+    return false;
+  }
+  bool node_found = false;
+  auto node_it = weak_nodes_.begin();
+  while (node_it != weak_nodes_.end()) {
+    bool matched = (node_it->lock() == node_ptr);
+    if (matched) {
+      weak_nodes_.erase(node_it);
+      node_found = true;
+      break;
+    }
+    ++node_it;
+  }
+  if (!node_found) {
+    return false;
+  }
+  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
+  std::for_each(
+    weak_groups_to_nodes_associated_with_executor_.begin(),
+    weak_groups_to_nodes_associated_with_executor_.end(),
+    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
+    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
+      auto & weak_node_ptr = key_value_pair.second;
+      auto shared_node_ptr = weak_node_ptr.lock();
+      auto group_ptr = key_value_pair.first.lock();
+      if (shared_node_ptr == node_ptr) {
+        found_group_ptrs.push_back(group_ptr);
+      }
+    });
+  std::for_each(
+    found_group_ptrs.begin(), found_group_ptrs.end(), [this]
+      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+      this->remove_callback_group_from_map(
+        group_ptr,
+        weak_groups_to_nodes_associated_with_executor_);
+    });
+  std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
+  has_executor.store(false);
+  return true;
+}
+
+// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
+bool
+EventsExecutorEntitiesCollector::has_node(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes) const
+{
+  return std::find_if(
+    weak_groups_to_nodes.begin(),
+    weak_groups_to_nodes.end(),
+    [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
+      auto other_ptr = other.second.lock();
+      return other_ptr == node_ptr;
+    }) != weak_groups_to_nodes.end();
+}
+
+void
+EventsExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
+{
+  for (const auto & weak_node : weak_nodes_) {
+    auto node = weak_node.lock();
+    if (node) {
+      node->for_each_callback_group(
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
+        {
+          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
+          !shared_group_ptr->get_associated_with_executor_atomic().load())
+          {
+            add_callback_group(
+              shared_group_ptr,
+              node,
+              weak_groups_to_nodes_associated_with_executor_);
+          }
+        });
+    }
+  }
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutorEntitiesCollector::get_all_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutorEntitiesCollector::get_manually_added_callback_groups()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+std::vector<rclcpp::CallbackGroup::WeakPtr>
+EventsExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
+{
+  std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+    groups.push_back(group_node_ptr.first);
+  }
+  return groups;
+}
+
+void
+EventsExecutorEntitiesCollector::callback_group_added_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group;
+
+  auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+  if (iter != weak_groups_to_guard_conditions_.end()) {
+    // Set an event callback for the group's notify guard condition, so if new entities are added
+    // or removed to this node we will receive an event.
+    set_guard_condition_callback(iter->second);
+  }
+  // For all entities in the callback group, set their event callback
+  set_callback_group_entities_callbacks(group);
+}
+
+void
+EventsExecutorEntitiesCollector::node_added_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto notify_guard_condition = &(node->get_notify_guard_condition());
+  // Set an event callback for the node's notify guard condition, so if new entities are added
+  // or removed to this node we will receive an event.
+  set_guard_condition_callback(notify_guard_condition);
+
+  // Store node's notify guard condition
+  weak_nodes_to_guard_conditions_[node] = notify_guard_condition;
+}
+
+void
+EventsExecutorEntitiesCollector::callback_group_removed_impl(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // For all the entities in the group, unset their callbacks
+  unset_callback_group_entities_callbacks(group);
+}
+
+void
+EventsExecutorEntitiesCollector::node_removed_impl(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // Node doesn't have more callback groups associated to the executor.
+  // Unset the event callback for the node's notify guard condition, to stop
+  // receiving events if entities are added or removed to this node.
+  unset_guard_condition_callback(&(node->get_notify_guard_condition()));
+
+  // Remove guard condition from list
+  rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr(node);
+  weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+}
+
+void
+EventsExecutorEntitiesCollector::set_entities_event_callbacks_from_map(
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+{
+  for (const auto & pair : weak_groups_to_nodes) {
+    auto group = pair.first.lock();
+    auto node = pair.second.lock();
+    if (!node || !group || !group->can_be_taken_from().load()) {
+      continue;
+    }
+    set_callback_group_entities_callbacks(group);
+  }
+}
+
+void
+EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  // Timers are handled by the timers manager
+  group->find_timer_ptrs_if(
+    [this](const rclcpp::TimerBase::SharedPtr & timer) {
+      if (timer) {
+        timers_manager_->add_timer(timer);
+      }
+      return false;
+    });
+
+  // Set callbacks for all other entity types
+  group->find_subscription_ptrs_if(
+    [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+      if (subscription) {
+        weak_subscriptions_map_.emplace(subscription.get(), subscription);
+
+        subscription->set_on_new_message_callback(
+          create_entity_callback(subscription.get(), ExecutorEventType::SUBSCRIPTION_EVENT));
+      }
+      return false;
+    });
+  group->find_service_ptrs_if(
+    [this](const rclcpp::ServiceBase::SharedPtr & service) {
+      if (service) {
+        weak_services_map_.emplace(service.get(), service);
+
+        service->set_on_new_request_callback(
+          create_entity_callback(service.get(), ExecutorEventType::SERVICE_EVENT));
+      }
+      return false;
+    });
+  group->find_client_ptrs_if(
+    [this](const rclcpp::ClientBase::SharedPtr & client) {
+      if (client) {
+        weak_clients_map_.emplace(client.get(), client);
+
+        client->set_on_new_response_callback(
+          create_entity_callback(client.get(), ExecutorEventType::CLIENT_EVENT));
+      }
+      return false;
+    });
+  group->find_waitable_ptrs_if(
+    [this](const rclcpp::Waitable::SharedPtr & waitable) {
+      if (waitable) {
+        weak_waitables_map_.emplace(waitable.get(), waitable);
+
+        waitable->set_on_ready_callback(
+          create_waitable_callback(waitable.get()));
+      }
+      return false;
+    });
+}
+
+void
+EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks(
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  auto iter = weak_groups_to_guard_conditions_.find(group);
+
+  if (iter != weak_groups_to_guard_conditions_.end()) {
+    unset_guard_condition_callback(iter->second);
+  }
+
+  // Timers are handled by the timers manager
+  group->find_timer_ptrs_if(
+    [this](const rclcpp::TimerBase::SharedPtr & timer) {
+      if (timer) {
+        timers_manager_->remove_timer(timer);
+      }
+      return false;
+    });
+
+  // Unset callbacks for all other entity types
+  group->find_subscription_ptrs_if(
+    [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
+      if (subscription) {
+        subscription->clear_on_new_message_callback();
+        weak_subscriptions_map_.erase(subscription.get());
+      }
+      return false;
+    });
+  group->find_service_ptrs_if(
+    [this](const rclcpp::ServiceBase::SharedPtr & service) {
+      if (service) {
+        service->clear_on_new_request_callback();
+        weak_services_map_.erase(service.get());
+      }
+      return false;
+    });
+  group->find_client_ptrs_if(
+    [this](const rclcpp::ClientBase::SharedPtr & client) {
+      if (client) {
+        client->clear_on_new_response_callback();
+        weak_clients_map_.erase(client.get());
+      }
+      return false;
+    });
+  group->find_waitable_ptrs_if(
+    [this](const rclcpp::Waitable::SharedPtr & waitable) {
+      if (waitable) {
+        waitable->clear_on_ready_callback();
+        weak_waitables_map_.erase(waitable.get());
+      }
+      return false;
+    });
+}
+
+void
+EventsExecutorEntitiesCollector::set_guard_condition_callback(
+  rclcpp::GuardCondition * guard_condition)
+{
+  auto gc_callback = [this](size_t num_events) {
+      // Override num events (we don't care more than a single event)
+      num_events = 1;
+      int gc_id = -1;
+      ExecutorEvent event = {this, gc_id, ExecutorEventType::WAITABLE_EVENT, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+
+  guard_condition->set_on_trigger_callback(gc_callback);
+}
+
+void
+EventsExecutorEntitiesCollector::unset_guard_condition_callback(
+  rclcpp::GuardCondition * guard_condition)
+{
+  guard_condition->set_on_trigger_callback(nullptr);
+}
+
+rclcpp::SubscriptionBase::SharedPtr
+EventsExecutorEntitiesCollector::get_subscription(const void * subscription_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_subscriptions_map_.find(subscription_id);
+
+  if (it != weak_subscriptions_map_.end()) {
+    auto subscription_weak_ptr = it->second;
+    auto subscription_shared_ptr = subscription_weak_ptr.lock();
+
+    if (subscription_shared_ptr) {
+      return subscription_shared_ptr;
+    }
+
+    // The subscription expired, remove from map
+    weak_subscriptions_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::ClientBase::SharedPtr
+EventsExecutorEntitiesCollector::get_client(const void * client_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_clients_map_.find(client_id);
+
+  if (it != weak_clients_map_.end()) {
+    auto client_weak_ptr = it->second;
+    auto client_shared_ptr = client_weak_ptr.lock();
+
+    if (client_shared_ptr) {
+      return client_shared_ptr;
+    }
+
+    // The client expired, remove from map
+    weak_clients_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::ServiceBase::SharedPtr
+EventsExecutorEntitiesCollector::get_service(const void * service_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_services_map_.find(service_id);
+
+  if (it != weak_services_map_.end()) {
+    auto service_weak_ptr = it->second;
+    auto service_shared_ptr = service_weak_ptr.lock();
+
+    if (service_shared_ptr) {
+      return service_shared_ptr;
+    }
+
+    // The service expired, remove from map
+    weak_services_map_.erase(it);
+  }
+  return nullptr;
+}
+
+rclcpp::Waitable::SharedPtr
+EventsExecutorEntitiesCollector::get_waitable(const void * waitable_id)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  auto it = weak_waitables_map_.find(waitable_id);
+
+  if (it != weak_waitables_map_.end()) {
+    auto waitable_weak_ptr = it->second;
+    auto waitable_shared_ptr = waitable_weak_ptr.lock();
+
+    if (waitable_shared_ptr) {
+      return waitable_shared_ptr;
+    }
+
+    // The waitable expired, remove from map
+    weak_waitables_map_.erase(it);
+  }
+  return nullptr;
+}
+
+void
+EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitable)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  weak_waitables_map_.emplace(waitable.get(), waitable);
+
+  waitable->set_on_ready_callback(
+    create_waitable_callback(waitable.get()));
+}
+
+std::function<void(size_t)>
+EventsExecutorEntitiesCollector::create_entity_callback(
+  void * exec_entity_id, ExecutorEventType event_type)
+{
+  std::function<void(size_t)>
+  callback = [this, exec_entity_id, event_type](size_t num_events) {
+      ExecutorEvent event = {exec_entity_id, -1, event_type, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+  return callback;
+}
+
+std::function<void(size_t, int)>
+EventsExecutorEntitiesCollector::create_waitable_callback(void * exec_entity_id)
+{
+  std::function<void(size_t, int)>
+  callback = [this, exec_entity_id](size_t num_events, int gen_entity_id) {
+      ExecutorEvent event =
+      {exec_entity_id, gen_entity_id, ExecutorEventType::WAITABLE_EVENT, num_events};
+      associated_executor_->events_queue_->enqueue(event);
+    };
+  return callback;
+}

rclcpp/src/rclcpp/experimental/timers_manager.cpp

@@ -0,0 +1,311 @@
+// 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>
+
+using rclcpp::experimental::TimersManager;
+
+TimersManager::TimersManager(
+  std::shared_ptr<rclcpp::Context> context,
+  std::function<void(void *)> 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();
+}
+
+void TimersManager::execute_ready_timers()
+{
+  // Do not allow to interfere with the thread running
+  if (running_) {
+    throw std::runtime_error(
+            "execute_ready_timers() can't be used while timers thread is running");
+  }
+
+  std::unique_lock<std::mutex> lock(timers_mutex_);
+  this->execute_ready_timers_unsafe();
+}
+
+bool TimersManager::execute_head_timer()
+{
+  // Do not allow to interfere with the thread running
+  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) {
+    head_timer->call();
+    if (on_ready_callback_) {
+      on_ready_callback_(head_timer.get());
+    } else {
+      head_timer->execute_callback();
+    }
+    timers_heap.heapify_root();
+    weak_timers_heap_.store(timers_heap);
+  }
+
+  return timer_ready;
+}
+
+void TimersManager::execute_ready_timer(const void * 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()
+{
+  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();
+  }
+
+  // Make sure the running flag is set to false when we exit from this function
+  // to allow restarting the timers thread.
+  running_ = false;
+}
+
+void TimersManager::clear()
+{
+  {
+    // 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_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/logger.cpp

@@ -12,14 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
 #include <string>
+#include <utility>
 
 #include "rcl_logging_interface/rcl_logging_interface.h"
+#include "rcl/logging_rosout.h"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 
+#include "./logging_mutex.hpp"
+
 namespace rclcpp
 {
 
@@ -62,6 +67,46 @@ get_logging_directory()
   return path;
 }
 
+Logger
+Logger::get_child(const std::string & suffix)
+{
+  if (!name_) {
+    return Logger();
+  }
+
+  rcl_ret_t rcl_ret = RCL_RET_OK;
+  std::shared_ptr<std::recursive_mutex> logging_mutex;
+  logging_mutex = get_global_logging_mutex();
+  {
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+    rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str());
+    if (RCL_RET_OK != rcl_ret) {
+      exceptions::throw_from_rcl_error(
+        rcl_ret, "failed to call rcl_logging_rosout_add_sublogger",
+        rcutils_get_error_state(), rcutils_reset_error);
+    }
+  }
+
+  Logger logger(*name_ + RCUTILS_LOGGING_SEPARATOR_STRING + suffix);
+  if (RCL_RET_OK == rcl_ret) {
+    logger.logger_sublogger_pairname_.reset(
+      new std::pair<std::string, std::string>({*name_, suffix}),
+      [logging_mutex](std::pair<std::string, std::string> * logger_sublogger_pairname_ptr) {
+        std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+        rcl_ret_t rcl_ret = rcl_logging_rosout_remove_sublogger(
+          logger_sublogger_pairname_ptr->first.c_str(),
+          logger_sublogger_pairname_ptr->second.c_str());
+        delete logger_sublogger_pairname_ptr;
+        if (RCL_RET_OK != rcl_ret) {
+          exceptions::throw_from_rcl_error(
+            rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger",
+            rcutils_get_error_state(), rcutils_reset_error);
+        }
+      });
+  }
+  return logger;
+}
+
 void
 Logger::set_level(Level level)
 {

rclcpp/src/rclcpp/node.cpp

@@ -176,7 +176,8 @@ Node::Node(
       node_topics_,
       node_graph_,
       node_services_,
-      node_logging_
+      node_logging_,
+      options.clock_type()
     )),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
       node_base_,

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -24,13 +24,14 @@ NodeClock::NodeClock(
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging)
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rcl_clock_type_t clock_type)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
   node_logging_(node_logging),
-  ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
+  clock_(std::make_shared<rclcpp::Clock>(clock_type))
 {}
 
 NodeClock::~NodeClock()
@@ -39,11 +40,11 @@ NodeClock::~NodeClock()
 rclcpp::Clock::SharedPtr
 NodeClock::get_clock()
 {
-  return ros_clock_;
+  return clock_;
 }
 
 rclcpp::Clock::ConstSharedPtr
 NodeClock::get_clock() const
 {
-  return ros_clock_;
+  return clock_;
 }

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -904,22 +904,12 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
   results.reserve(names.size());
 
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   for (auto & name : names) {
-    auto found_parameter = parameters_.find(name);
-    if (found_parameter != parameters_.cend()) {
-      // found
-      results.emplace_back(name, found_parameter->second.value);
-    } else if (this->allow_undeclared_) {
-      // not found, but undeclared allowed
-      results.emplace_back(name, rclcpp::ParameterValue());
-    } else {
-      // not found, and undeclared are not allowed
-      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
-    }
+    results.emplace_back(this->get_parameter(name));
   }
   return results;
 }
@@ -930,18 +920,18 @@ NodeParameters::get_parameter(const std::string & name) const
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   auto param_iter = parameters_.find(name);
-  if (
-    parameters_.end() != param_iter &&
-    (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET ||
-    param_iter->second.descriptor.dynamic_typing))
-  {
-    return rclcpp::Parameter{name, param_iter->second.value};
-  } else if (this->allow_undeclared_) {
-    return rclcpp::Parameter{};
-  } else if (parameters_.end() == param_iter) {
-    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
-  } else {
+  if (parameters_.end() != param_iter) {
+    if (
+      param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET ||
+      param_iter->second.descriptor.dynamic_typing)
+    {
+      return rclcpp::Parameter{name, param_iter->second.value};
+    }
     throw rclcpp::exceptions::ParameterUninitializedException(name);
+  } else if (this->allow_undeclared_) {
+    return rclcpp::Parameter{name};
+  } else {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
   }
 }
 

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -35,15 +35,17 @@ NodeServices::add_service(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create service, group not in node.");
     }
-    group->add_service(service_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_service(service_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  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();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on service creation: ") + ex.what());
@@ -60,15 +62,17 @@ NodeServices::add_client(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create client, group not in node.");
     }
-    group->add_client(client_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_client(client_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  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();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on client creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -37,14 +37,15 @@ NodeTimers::add_timer(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create timer, group not in node.");
     }
-    callback_group->add_timer(timer);
   } else {
-    node_base_->get_default_callback_group()->add_timer(timer);
+    callback_group = node_base_->get_default_callback_group();
   }
+  callback_group->add_timer(timer);
 
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on timer creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -73,6 +73,7 @@ NodeTopics::add_publisher(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on publisher creation: ") + ex.what());
@@ -121,6 +122,7 @@ NodeTopics::add_subscription(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on subscription creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -35,15 +35,17 @@ NodeWaitables::add_waitable(
       // TODO(jacobperron): use custom exception
       throw std::runtime_error("Cannot create waitable, group not in node.");
     }
-    group->add_waitable(waitable_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  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();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on waitable creation: ") + ex.what());

rclcpp/src/rclcpp/node_options.cpp

@@ -77,6 +77,7 @@ NodeOptions::operator=(const NodeOptions & other)
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
     this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->clock_type_ = other.clock_type_;
     this->clock_qos_ = other.clock_qos_;
     this->use_clock_thread_ = other.use_clock_thread_;
     this->parameter_event_qos_ = other.parameter_event_qos_;
@@ -260,6 +261,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rcl_clock_type_t &
+NodeOptions::clock_type() const
+{
+  return this->clock_type_;
+}
+
+NodeOptions &
+NodeOptions::clock_type(const rcl_clock_type_t & clock_type)
+{
+  this->clock_type_ = clock_type;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::clock_qos() const
 {

rclcpp/src/rclcpp/parameter_service.cpp

@@ -48,6 +48,8 @@ ParameterService::ParameterService(
         }
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
         RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
+      } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) {
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/publisher_base.cpp

@@ -37,7 +37,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/node.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 
 using rclcpp::PublisherBase;
 
@@ -45,11 +45,14 @@ PublisherBase::PublisherBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
   const std::string & topic,
   const rosidl_message_type_support_t & type_support,
-  const rcl_publisher_options_t & publisher_options)
+  const rcl_publisher_options_t & publisher_options,
+  const PublisherEventCallbacks & event_callbacks,
+  bool use_default_callbacks)
 : rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
   intra_process_is_enabled_(false),
   intra_process_publisher_id_(0),
-  type_support_(type_support)
+  type_support_(type_support),
+  event_callbacks_(event_callbacks)
 {
   auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub)
     {
@@ -98,15 +101,12 @@ PublisherBase::PublisherBase(
     rmw_reset_error();
     throw std::runtime_error(msg);
   }
+
+  bind_event_callbacks(event_callbacks_, use_default_callbacks);
 }
 
 PublisherBase::~PublisherBase()
 {
-  for (const auto & pair : event_handlers_) {
-    rcl_publisher_event_type_t event_type = pair.first;
-    clear_on_new_qos_event_callback(event_type);
-  }
-
   // must fini the events before fini-ing the publisher
   event_handlers_.clear();
 
@@ -131,6 +131,65 @@ PublisherBase::get_topic_name() const
   return rcl_publisher_get_topic_name(publisher_handle_.get());
 }
 
+void
+PublisherBase::bind_event_callbacks(
+  const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks)
+{
+  if (event_callbacks.deadline_callback) {
+    this->add_event_handler(
+      event_callbacks.deadline_callback,
+      RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+  }
+  if (event_callbacks.liveliness_callback) {
+    this->add_event_handler(
+      event_callbacks.liveliness_callback,
+      RCL_PUBLISHER_LIVELINESS_LOST);
+  }
+
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb;
+  if (event_callbacks.incompatible_qos_callback) {
+    incompatible_qos_cb = event_callbacks.incompatible_qos_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_qos_cb = [this](QOSOfferedIncompatibleQoSInfo & info) {
+        this->default_incompatible_qos_callback(info);
+      };
+  }
+  try {
+    if (incompatible_qos_cb) {
+      this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible qos; wrong callback type");
+  }
+
+  IncompatibleTypeCallbackType incompatible_type_cb;
+  if (event_callbacks.incompatible_type_callback) {
+    incompatible_type_cb = event_callbacks.incompatible_type_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_type_cb = [this](IncompatibleTypeInfo & info) {
+        this->default_incompatible_type_callback(info);
+      };
+  }
+  try {
+    if (incompatible_type_cb) {
+      this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE);
+    }
+  } catch (UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible type; wrong callback type");
+  }
+  if (event_callbacks.matched_callback) {
+    this->add_event_handler(
+      event_callbacks.matched_callback,
+      RCL_PUBLISHER_MATCHED);
+  }
+}
+
 size_t
 PublisherBase::get_queue_size() const
 {
@@ -163,7 +222,7 @@ PublisherBase::get_publisher_handle() const
 }
 
 const
-std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
 PublisherBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -233,7 +292,7 @@ PublisherBase::assert_liveliness() const
 bool
 PublisherBase::can_loan_messages() const
 {
-  return rcl_publisher_can_loan_messages(publisher_handle_.get());
+  return !intra_process_is_enabled_ && rcl_publisher_can_loan_messages(publisher_handle_.get());
 }
 
 bool
@@ -279,6 +338,17 @@ PublisherBase::default_incompatible_qos_callback(
     policy_name.c_str());
 }
 
+void
+PublisherBase::default_incompatible_type_callback(
+  rclcpp::IncompatibleTypeInfo & event) const
+{
+  (void)event;
+
+  RCLCPP_WARN(
+    rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())),
+    "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name());
+}
+
 std::vector<rclcpp::NetworkFlowEndpoint> PublisherBase::get_network_flow_endpoints() const
 {
   rcutils_allocator_t allocator = rcutils_get_default_allocator();

rclcpp/src/rclcpp/qos.cpp

@@ -16,6 +16,8 @@
 
 #include <string>
 
+#include "rclcpp/logging.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/types.h"
 #include "rmw/qos_profiles.h"
@@ -43,9 +45,19 @@ std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind)
   }
 }
 
-QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg)
+QoSInitialization::QoSInitialization(
+  rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
+  bool print_depth_warning)
 : history_policy(history_policy_arg), depth(depth_arg)
-{}
+{
+  if (history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && depth == 0 && print_depth_warning) {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored. "
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
+}
 
 QoSInitialization
 QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos)
@@ -53,8 +65,9 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos)
   switch (rmw_qos.history) {
     case RMW_QOS_POLICY_HISTORY_KEEP_ALL:
       return KeepAll();
-    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
     case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
+      return KeepLast(rmw_qos.depth, false);
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
     case RMW_QOS_POLICY_HISTORY_UNKNOWN:
     default:
       return KeepLast(rmw_qos.depth);
@@ -65,9 +78,10 @@ KeepAll::KeepAll()
 : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0)
 {}
 
-KeepLast::KeepLast(size_t depth)
-: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth)
-{}
+KeepLast::KeepLast(size_t depth, bool print_depth_warning)
+: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth, print_depth_warning)
+{
+}
 
 QoS::QoS(
   const QoSInitialization & qos_initialization,
@@ -111,6 +125,14 @@ QoS::history(HistoryPolicy history)
 QoS &
 QoS::keep_last(size_t depth)
 {
+  if (depth == 0) {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
+
   rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
   rmw_qos_profile_.depth = depth;
   return *this;

rclcpp/src/rclcpp/service.cpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -32,10 +33,6 @@ ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
   node_logger_(rclcpp::get_node_logger(node_handle_.get()))
 {}
 
-ServiceBase::~ServiceBase()
-{
-  clear_on_new_request_callback();
-}
 
 bool
 ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)
@@ -135,7 +132,7 @@ ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const vo
     user_data);
 
   if (RCL_RET_OK != ret) {
-    using rclcpp::exceptions::throw_from_rcl_error;
-    throw_from_rcl_error(ret, "failed to set the on new request callback for service");
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to set the on new request callback for service");
   }
 }

rclcpp/src/rclcpp/signal_handler.cpp

@@ -191,7 +191,9 @@ SignalHandler::uninstall()
     signal_handlers_options_ = SignalHandlerOptions::None;
     RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler");
     notify_signal_handler();
-    signal_handler_thread_.join();
+    if (signal_handler_thread_.joinable()) {
+      signal_handler_thread_.join();
+    }
     teardown_wait_for_signal();
   } catch (...) {
     installed_.exchange(true);

rclcpp/src/rclcpp/subscription_base.cpp

@@ -27,7 +27,7 @@
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -39,12 +39,15 @@ SubscriptionBase::SubscriptionBase(
   const rosidl_message_type_support_t & type_support_handle,
   const std::string & topic_name,
   const rcl_subscription_options_t & subscription_options,
+  const SubscriptionEventCallbacks & event_callbacks,
+  bool use_default_callbacks,
   bool is_serialized)
 : node_base_(node_base),
   node_handle_(node_base_->get_shared_rcl_node_handle()),
   node_logger_(rclcpp::get_node_logger(node_handle_.get())),
   use_intra_process_(false),
   intra_process_subscription_id_(0),
+  event_callbacks_(event_callbacks),
   type_support_(type_support_handle),
   is_serialized_(is_serialized)
 {
@@ -80,20 +83,14 @@ SubscriptionBase::SubscriptionBase(
         rcl_node_get_name(rcl_node_handle),
         rcl_node_get_namespace(rcl_node_handle));
     }
-
     rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription");
   }
+
+  bind_event_callbacks(event_callbacks_, use_default_callbacks);
 }
 
 SubscriptionBase::~SubscriptionBase()
 {
-  clear_on_new_message_callback();
-
-  for (const auto & pair : event_handlers_) {
-    rcl_subscription_event_type_t event_type = pair.first;
-    clear_on_new_qos_event_callback(event_type);
-  }
-
   if (!use_intra_process_) {
     return;
   }
@@ -108,6 +105,69 @@ SubscriptionBase::~SubscriptionBase()
   ipm->remove_subscription(intra_process_subscription_id_);
 }
 
+void
+SubscriptionBase::bind_event_callbacks(
+  const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks)
+{
+  if (event_callbacks.deadline_callback) {
+    this->add_event_handler(
+      event_callbacks.deadline_callback,
+      RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
+  }
+
+  if (event_callbacks.liveliness_callback) {
+    this->add_event_handler(
+      event_callbacks.liveliness_callback,
+      RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
+  }
+
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb;
+  if (event_callbacks.incompatible_qos_callback) {
+    incompatible_qos_cb = event_callbacks.incompatible_qos_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_qos_cb = [this](QOSRequestedIncompatibleQoSInfo & info) {
+        this->default_incompatible_qos_callback(info);
+      };
+  }
+  // Register default callback when not specified
+  try {
+    if (incompatible_qos_cb) {
+      this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    // pass
+  }
+
+  IncompatibleTypeCallbackType incompatible_type_cb;
+  if (event_callbacks.incompatible_type_callback) {
+    incompatible_type_cb = event_callbacks.incompatible_type_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_type_cb = [this](IncompatibleTypeInfo & info) {
+        this->default_incompatible_type_callback(info);
+      };
+  }
+  try {
+    if (incompatible_type_cb) {
+      this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE);
+    }
+  } catch (UnsupportedEventTypeException & /*exc*/) {
+    // pass
+  }
+
+  if (event_callbacks.message_lost_callback) {
+    this->add_event_handler(
+      event_callbacks.message_lost_callback,
+      RCL_SUBSCRIPTION_MESSAGE_LOST);
+  }
+  if (event_callbacks.matched_callback) {
+    this->add_event_handler(
+      event_callbacks.matched_callback,
+      RCL_SUBSCRIPTION_MATCHED);
+  }
+}
+
 const char *
 SubscriptionBase::get_topic_name() const
 {
@@ -127,7 +187,7 @@ SubscriptionBase::get_subscription_handle() const
 }
 
 const
-std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
 SubscriptionBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -265,6 +325,17 @@ SubscriptionBase::default_incompatible_qos_callback(
     policy_name.c_str());
 }
 
+void
+SubscriptionBase::default_incompatible_type_callback(
+  rclcpp::IncompatibleTypeInfo & event) const
+{
+  (void)event;
+
+  RCLCPP_WARN(
+    rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())),
+    "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name());
+}
+
 bool
 SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const
 {

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -17,11 +17,6 @@
 
 using rclcpp::experimental::SubscriptionIntraProcessBase;
 
-SubscriptionIntraProcessBase::~SubscriptionIntraProcessBase()
-{
-  clear_on_ready_callback();
-}
-
 void
 SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {

rclcpp/src/rclcpp/time_source.cpp

@@ -37,7 +37,8 @@ class ClocksState final
 {
 public:
   ClocksState()
-  : logger_(rclcpp::get_logger("rclcpp"))
+  : logger_(rclcpp::get_logger("rclcpp")),
+    last_time_msg_(std::make_shared<builtin_interfaces::msg::Time>())
   {
   }
 
@@ -53,13 +54,8 @@ public:
     ros_time_active_ = true;
 
     // Update all attached clocks to zero or last recorded time
-    std::lock_guard<std::mutex> guard(clock_list_lock_);
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
-    if (last_msg_set_) {
-      time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    }
     for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-      set_clock(time_msg, true, *it);
+      set_clock(last_time_msg_, true, *it);
     }
   }
 
@@ -91,18 +87,17 @@ public:
   // Attach a clock
   void attachClock(rclcpp::Clock::SharedPtr clock)
   {
-    if (clock->get_clock_type() != RCL_ROS_TIME) {
-      throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock");
+    {
+      std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+      if (clock->get_clock_type() != RCL_ROS_TIME && ros_time_active_) {
+        throw std::invalid_argument(
+                "ros_time_active_ can't be true while clock is not of RCL_ROS_TIME type");
+      }
     }
-
     std::lock_guard<std::mutex> guard(clock_list_lock_);
     associated_clocks_.push_back(clock);
     // Set the clock to zero unless there's a recently received message
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
-    if (last_msg_set_) {
-      time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    }
-    set_clock(time_msg, ros_time_active_, clock);
+    set_clock(last_time_msg_, ros_time_active_, clock);
   }
 
   // Detach a clock
@@ -125,27 +120,32 @@ public:
   {
     std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
 
-    // Do change
-    if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-      auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-      if (ret != RCL_RET_OK) {
-        rclcpp::exceptions::throw_from_rcl_error(
-          ret, "Failed to disable ros_time_override_status");
+    if (clock->get_clock_type() == RCL_ROS_TIME) {
+      // Do change
+      if (!set_ros_time_enabled && clock->ros_time_is_active()) {
+        auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+        if (ret != RCL_RET_OK) {
+          rclcpp::exceptions::throw_from_rcl_error(
+            ret, "Failed to disable ros_time_override_status");
+        }
+      } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
+        auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+        if (ret != RCL_RET_OK) {
+          rclcpp::exceptions::throw_from_rcl_error(
+            ret, "Failed to enable ros_time_override_status");
+        }
       }
-    } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-      auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-      if (ret != RCL_RET_OK) {
-        rclcpp::exceptions::throw_from_rcl_error(
-          ret, "Failed to enable ros_time_override_status");
-      }
-    }
 
-    auto ret = rcl_set_ros_time_override(
-      clock->get_clock_handle(),
-      rclcpp::Time(*msg).nanoseconds());
-    if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret, "Failed to set ros_time_override_status");
+      auto ret = rcl_set_ros_time_override(
+        clock->get_clock_handle(),
+        rclcpp::Time(*msg).nanoseconds());
+      if (ret != RCL_RET_OK) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "Failed to set ros_time_override_status");
+      }
+    } else if (set_ros_time_enabled) {
+      throw std::invalid_argument(
+              "set_ros_time_enabled can't be true while clock is not of RCL_ROS_TIME type");
     }
   }
 
@@ -163,7 +163,19 @@ public:
   // Cache the last clock message received
   void cache_last_msg(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
   {
-    last_msg_set_ = msg;
+    last_time_msg_ = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
+  }
+
+  bool are_all_clocks_rcl_ros_time()
+  {
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    for (auto & clock : associated_clocks_) {
+      std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+      if (clock->get_clock_type() != RCL_ROS_TIME) {
+        return false;
+      }
+    }
+    return true;
   }
 
 private:
@@ -179,7 +191,7 @@ private:
   // This is needed when new clocks are added.
   bool ros_time_active_{false};
   // Last set message to be passed to newly registered clocks
-  std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
+  std::shared_ptr<builtin_interfaces::msg::Time> last_time_msg_{nullptr};
 };
 
 class TimeSource::NodeState final
@@ -266,6 +278,10 @@ public:
       throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'");
     }
 
+    on_set_parameters_callback_ = node_parameters_->add_on_set_parameters_callback(
+      std::bind(&TimeSource::NodeState::on_set_parameters, this, std::placeholders::_1));
+
+
     // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
     parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
       node_topics_,
@@ -285,6 +301,10 @@ public:
     // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
     clocks_state_.disable_ros_time();
+    if (on_set_parameters_callback_) {
+      node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get());
+    }
+    on_set_parameters_callback_.reset();
     parameter_subscription_.reset();
     node_base_.reset();
     node_topics_.reset();
@@ -419,10 +439,34 @@ private:
     clock_subscription_.reset();
   }
 
+  // On set Parameters callback handle
+  node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr};
+
   // Parameter Event subscription
   using ParamSubscriptionT = rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>;
   std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
 
+  // Callback for parameter settings
+  rcl_interfaces::msg::SetParametersResult on_set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    for (const auto & param : parameters) {
+      if (param.get_name() == "use_sim_time" && param.get_type() == rclcpp::PARAMETER_BOOL) {
+        if (param.as_bool() && !(clocks_state_.are_all_clocks_rcl_ros_time())) {
+          result.successful = false;
+          result.reason =
+            "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type";
+          RCLCPP_ERROR(
+            logger_,
+            "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type");
+        }
+      }
+    }
+    return result;
+  }
+
   // Callback for parameter updates
   void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
   {

rclcpp/src/rclcpp/timer.cpp

@@ -19,9 +19,12 @@
 #include <memory>
 #include <thread>
 
-#include "rclcpp/contexts/default_context.hpp"
-#include "rclcpp/exceptions.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
 
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 #include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
@@ -71,7 +74,9 @@ TimerBase::TimerBase(
 }
 
 TimerBase::~TimerBase()
-{}
+{
+  clear_on_reset_callback();
+}
 
 void
 TimerBase::cancel()
@@ -96,7 +101,11 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
+  rcl_ret_t ret = RCL_RET_OK;
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    ret = rcl_timer_reset(timer_handle_.get());
+  }
   if (ret != RCL_RET_OK) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
   }
@@ -138,3 +147,73 @@ TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+TimerBase::set_on_reset_callback(std::function<void(size_t)> callback)
+{
+  if (!callback) {
+    throw std::invalid_argument(
+            "The callback passed to set_on_reset_callback "
+            "is not callable.");
+  }
+
+  auto new_callback =
+    [callback, this](size_t reset_calls) {
+      try {
+        callback(reset_calls);
+      } catch (const std::exception & exception) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("rclcpp"),
+          "rclcpp::TimerBase@" << this <<
+            " caught " << rmw::impl::cpp::demangle(exception) <<
+            " exception in user-provided callback for the 'on reset' callback: " <<
+            exception.what());
+      } catch (...) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("rclcpp"),
+          "rclcpp::TimerBase@" << this <<
+            " caught unhandled exception in user-provided callback " <<
+            "for the 'on reset' callback");
+      }
+    };
+
+  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+  // 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 rcl hasn't been told about the new one yet.
+  set_on_reset_callback(
+    rclcpp::detail::cpp_callback_trampoline<
+      decltype(new_callback), const void *, size_t>,
+    static_cast<const void *>(&new_callback));
+
+  // Store the std::function to keep it in scope, also overwrites the existing one.
+  on_reset_callback_ = new_callback;
+
+  // Set it again, now using the permanent storage.
+  set_on_reset_callback(
+    rclcpp::detail::cpp_callback_trampoline<
+      decltype(on_reset_callback_), const void *, size_t>,
+    static_cast<const void *>(&on_reset_callback_));
+}
+
+void
+TimerBase::clear_on_reset_callback()
+{
+  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+  if (on_reset_callback_) {
+    set_on_reset_callback(nullptr, nullptr);
+    on_reset_callback_ = nullptr;
+  }
+}
+
+void
+TimerBase::set_on_reset_callback(rcl_event_callback_t callback, const void * user_data)
+{
+  rcl_ret_t ret = rcl_timer_set_on_reset_callback(timer_handle_.get(), callback, user_data);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer on reset callback");
+  }
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -100,15 +100,20 @@ if(TARGET test_create_subscription)
     "test_msgs"
   )
 endif()
-ament_add_gtest(test_add_callback_groups_to_executor
-  test_add_callback_groups_to_executor.cpp
-  TIMEOUT 120)
-if(TARGET test_add_callback_groups_to_executor)
-  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
-  ament_target_dependencies(test_add_callback_groups_to_executor
-    "test_msgs"
+function(test_add_callback_groups_to_executor_for_rmw_implementation)
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+  ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp
+    ENV ${rmw_implementation_env_var}
+    TIMEOUT 120
   )
-endif()
+  if(TARGET test_add_callback_groups_to_executor${target_suffix})
+    target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME})
+    ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix}
+      "test_msgs"
+    )
+  endif()
+endfunction()
+call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation)
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
 if(TARGET test_expand_topic_or_service_name)
   ament_target_dependencies(test_expand_topic_or_service_name
@@ -229,6 +234,11 @@ if(TARGET test_node_interfaces__node_graph)
     "test_msgs")
   target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__node_interfaces
+  node_interfaces/test_node_interfaces.cpp)
+if(TARGET test_node_interfaces__node_interfaces)
+  target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick)
+endif()
 ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
 if(TARGET test_node_interfaces__node_parameters)
@@ -257,6 +267,11 @@ ament_add_gtest(test_node_interfaces__node_waitables
 if(TARGET test_node_interfaces__node_waitables)
   target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__test_template_utils  # Compile time test
+  node_interfaces/detail/test_template_utils.cpp)
+if(TARGET test_node_interfaces__test_template_utils)
+  target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME})
+endif()
 
 # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
 # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
@@ -429,17 +444,23 @@ function(test_generic_pubsub_for_rmw_implementation)
   endif()
 endfunction()
 call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation)
-ament_add_gtest(test_qos_event test_qos_event.cpp)
-if(TARGET test_qos_event)
-  ament_target_dependencies(test_qos_event
-    "rmw"
-    "test_msgs"
+
+function(test_qos_event_for_rmw_implementation)
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+  ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp
+    ENV ${rmw_implementation_env_var}
   )
-  target_link_libraries(test_qos_event
-    ${PROJECT_NAME}
-    mimick
-  )
-endif()
+  if(TARGET test_qos_event${target_suffix})
+    target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick)
+    ament_target_dependencies(test_qos_event${target_suffix}
+      "rmw"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+  endif()
+endfunction()
+call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation)
+
 ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp)
 if(TARGET test_qos_overriding_options)
   target_link_libraries(test_qos_overriding_options
@@ -493,6 +514,17 @@ if(TARGET test_service)
   )
   target_link_libraries(test_service ${PROJECT_NAME} mimick)
 endif()
+ament_add_gmock(test_service_introspection test_service_introspection.cpp)
+if(TARGET test_service)
+  ament_target_dependencies(test_service_introspection
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick)
+endif()
 # Creating and destroying nodes is slow with Connext, so this needs larger timeout.
 ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
 if(TARGET test_subscription)
@@ -567,6 +599,9 @@ target_link_libraries(test_logger ${PROJECT_NAME})
 ament_add_gmock(test_logging test_logging.cpp)
 target_link_libraries(test_logging ${PROJECT_NAME})
 
+ament_add_gmock(test_context test_context.cpp)
+target_link_libraries(test_context ${PROJECT_NAME})
+
 ament_add_gtest(test_time test_time.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_time)
@@ -583,6 +618,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)
@@ -653,6 +696,22 @@ if(TARGET test_static_executor_entities_collector)
   target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
 endif()
 
+ament_add_gtest(test_events_executor executors/test_events_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_events_executor)
+  ament_target_dependencies(test_events_executor
+    "test_msgs")
+  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)
@@ -721,6 +780,12 @@ if(TARGET test_rosout_qos)
   target_link_libraries(test_rosout_qos ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp)
+if(TARGET test_rosout_subscription)
+  ament_target_dependencies(test_rosout_subscription "rcl")
+  target_link_libraries(test_rosout_subscription ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_executor test_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 120)

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -51,6 +51,53 @@ TEST(TestAllocatorCommon, retyped_allocate) {
   EXPECT_NO_THROW(code2());
 }
 
+TEST(TestAllocatorCommon, retyped_zero_allocate_basic) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
+  ASSERT_TRUE(nullptr != allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<char, std::allocator<char>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+}
+
+TEST(TestAllocatorCommon, retyped_zero_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+
+  allocated_mem = allocator.allocate(1);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != reallocated_mem);
+
+  auto code2 = [&untyped_allocator, reallocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        reallocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code2());
+}
+
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
   auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);

rclcpp/test/rclcpp/executors/test_events_executor.cpp

@@ -0,0 +1,499 @@
+// 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_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);
+}
+
+// FIX THIS TEST! The entities collector is being called too many times!
+/*
+TEST_F(TestEventsExecutor, spin_once_max_duration_timer)
+{
+  // rmw_connextdds doesn't support events-executor
+  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)
+{
+  // 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.exec_entity_id, event.exec_entity_id);
+  EXPECT_EQ(push_event.gen_entity_id, event.gen_entity_id);
+  EXPECT_EQ(push_event.type, event.type);
+  EXPECT_EQ(push_event.num_events, event.num_events);
+}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -92,7 +92,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 +114,10 @@ public:
       return "StaticSingleThreadedExecutor";
     }
 
+    if (std::is_same<T, rclcpp::experimental::executors::EventsExecutor>()) {
+      return "EventsExecutor";
+    }
+
     return "";
   }
 };
@@ -126,12 +131,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 +160,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;
 
   {
@@ -164,8 +188,17 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) {
 }
 
 // Check executor throws properly if the same node is added a second time
-TYPED_TEST(TestExecutors, addNodeTwoExecutors) {
+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 +207,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 +238,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 +273,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 +304,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 +336,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 +391,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 +467,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 +482,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 +511,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 +561,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>();
@@ -483,8 +610,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 +634,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 +658,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);
 
@@ -556,7 +710,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) {
 }
 
 // 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 +731,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);
 
   {

rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp

@@ -0,0 +1,56 @@
+// Copyright 2022 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/detail/template_contains.hpp"
+#include "rclcpp/detail/template_unique.hpp"
+
+
+TEST(NoOpTests, test_node_interfaces_template_utils) {
+}                                                        // This is just to let gtest work
+
+namespace rclcpp
+{
+namespace detail
+{
+
+// This tests template logic at compile time
+namespace
+{
+
+struct test_template_unique
+{
+  static_assert(template_unique_v<int>, "failed");
+  static_assert(template_unique_v<int, double>, "failed");
+  static_assert(!template_unique_v<int, int>, "failed");
+  static_assert(!template_unique_v<int, double, int>, "failed");
+  static_assert(!template_unique_v<int, int, double>, "failed");
+};
+
+
+struct test_template_contains
+{
+  static_assert(template_contains_v<int, int, double>, "failed");
+  static_assert(template_contains_v<double, int, double>, "failed");
+  static_assert(template_contains_v<int, int>, "failed");
+  static_assert(!template_contains_v<int>, "failed");
+  static_assert(!template_contains_v<int, float>, "failed");
+  static_assert(!template_contains_v<int, float, double>, "failed");
+};
+
+}   // namespace
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp

@@ -0,0 +1,254 @@
+// Copyright 2022 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_interfaces.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+
+class TestNodeInterfaces : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestNodeInterfaces, default_constructor) {
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  NodeInterfaces<NodeBaseInterface, NodeGraphInterface> interfaces;
+  interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(*node);
+}
+
+/*
+   Testing NodeInterfaces construction from nodes.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+
+  // Create a NodeInterfaces for base and graph using a rclcpp::Node.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto node_interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(*node);
+  }
+
+  // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni) {
+        auto base_interface = ni.get<NodeBaseInterface>();
+      };
+
+    some_func(*node);
+  }
+
+  // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni_with_one) {
+        auto base_interface = ni_with_one.get<NodeBaseInterface>();
+      };
+
+    NodeInterfaces<NodeBaseInterface, NodeGraphInterface> ni_with_two(*node);
+
+    some_func(ni_with_two);
+  }
+
+  // Create a NodeInterfaces via aggregation of interfaces in constructor.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto loose_node_base = node->get_node_base_interface();
+    auto loose_node_graph = node->get_node_graph_interface();
+    auto ni = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(
+      loose_node_base,
+      loose_node_graph);
+  }
+}
+
+/*
+   Test construction with all standard rclcpp::node_interfaces::Node*Interfaces.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<
+    rclcpp::node_interfaces::NodeBaseInterface,
+    rclcpp::node_interfaces::NodeClockInterface,
+    rclcpp::node_interfaces::NodeGraphInterface,
+    rclcpp::node_interfaces::NodeLoggingInterface,
+    rclcpp::node_interfaces::NodeTimersInterface,
+    rclcpp::node_interfaces::NodeTopicsInterface,
+    rclcpp::node_interfaces::NodeServicesInterface,
+    rclcpp::node_interfaces::NodeWaitablesInterface,
+    rclcpp::node_interfaces::NodeParametersInterface,
+    rclcpp::node_interfaces::NodeTimeSourceInterface
+    >(*node);
+}
+
+/*
+   Testing getters.
+ */
+TEST_F(TestNodeInterfaces, ni_init) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = NodeInterfaces<
+    NodeBaseInterface,
+    NodeClockInterface,
+    NodeGraphInterface,
+    NodeLoggingInterface,
+    NodeTimersInterface,
+    NodeTopicsInterface,
+    NodeServicesInterface,
+    NodeWaitablesInterface,
+    NodeParametersInterface,
+    NodeTimeSourceInterface
+    >(*node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}
+
+/*
+   Testing macro'ed getters.
+ */
+TEST_F(TestNodeInterfaces, ni_all_init) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(*node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}