Add in dispatch_intra_process for SubscribedTypes.

Not used yet, but at least the general infrastructure is there.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,71 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+13.1.0 (2021-10-18)
+-------------------
+* Fix dangerous std::bind capture in TimeSource implementation. (`#1768 <https://github.com/ros2/rclcpp/issues/1768>`_)
+* Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 <https://github.com/ros2/rclcpp/issues/1770>`_)
+* Handle sigterm, in the same way sigint is being handled. (`#1771 <https://github.com/ros2/rclcpp/issues/1771>`_)
+* rclcpp::Node copy constructor: make copy of node_waitables\_ member. (`#1799 <https://github.com/ros2/rclcpp/issues/1799>`_)
+* Extend NodeGraph to match what rcl provides. (`#1484 <https://github.com/ros2/rclcpp/issues/1484>`_)
+* Context::sleep_for(): replace recursion with do-while to avoid potential stack-overflow. (`#1765 <https://github.com/ros2/rclcpp/issues/1765>`_)
+* extend_sub_namespace(): Verify string::empty() before calling string::front(). (`#1764 <https://github.com/ros2/rclcpp/issues/1764>`_)
+* Deprecate the `void shared_ptr<MessageT>` subscription callback signatures. (`#1713 <https://github.com/ros2/rclcpp/issues/1713>`_)
+* Contributors: Abrar Rahman Protyasha, Chris Lalancette, Emerson Knapp, Geoffrey Biggs, Ivan Santiago Paunovic, Jorge Perez, Tomoya Fujita, William Woodall, Yong-Hao Zou, livanov93
+
+13.0.0 (2021-08-23)
+-------------------
+* Remove can_be_nullptr assignment check for QNX case. (`#1752 <https://github.com/ros2/rclcpp/issues/1752>`_)
+* Update client API to be able to remove pending requests. (`#1734 <https://github.com/ros2/rclcpp/issues/1734>`_)
+* Fix: Allow to add a node while spinning in the StaticSingleThreadedExecutor. (`#1690 <https://github.com/ros2/rclcpp/issues/1690>`_)
+* Add tracing instrumentation for executor and message taking. (`#1738 <https://github.com/ros2/rclcpp/issues/1738>`_)
+* Fix: Reset timer trigger time before execute in StaticSingleThreadedExecutor. (`#1739 <https://github.com/ros2/rclcpp/issues/1739>`_)
+* Use FindPython3 and make python3 dependency explicit. (`#1745 <https://github.com/ros2/rclcpp/issues/1745>`_)
+* Use rosidl_get_typesupport_target(). (`#1729 <https://github.com/ros2/rclcpp/issues/1729>`_)
+* Fix returning invalid namespace if sub_namespace is empty. (`#1658 <https://github.com/ros2/rclcpp/issues/1658>`_)
+* Add free function to wait for a subscription message. (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_)
+* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 <https://github.com/ros2/rclcpp/issues/1727>`_)
+* Contributors: Ahmed Sobhy, Christophe Bedard, Ivan Santiago Paunovic, Karsten Knese, M. Hofstätter, Mauro Passerino, Shane Loretz, mauropasse
+
+12.0.0 (2021-07-26)
+-------------------
+* Remove unsafe get_callback_groups API.
+  Callers should change to using for_each_callback_group(), or
+  store the callback groups they need internally.
+* Add in callback_groups_for_each.
+  The main reason to add this method in is to make accesses to the
+  callback_groups\_ vector thread-safe.  By having a
+  callback_groups_for_each that accepts a std::function, we can
+  just have the callers give us the callback they are interested
+  in, and we can take care of the locking.
+  The rest of this fairly large PR is cleaning up all of the places
+  that use get_callback_groups() to instead use
+  callback_groups_for_each().
+* Use a different mechanism to avoid timers being scheduled multiple times by the MultiThreadedExecutor (`#1692 <https://github.com/ros2/rclcpp/issues/1692>`_)
+* Fix windows CI (`#1726 <https://github.com/ros2/rclcpp/issues/1726>`_)
+  Fix bug in AnyServiceCallback introduced in `#1709 <https://github.com/ros2/rclcpp/issues/1709>`_.
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic
+
+11.2.0 (2021-07-21)
+-------------------
+* Support to defer to send a response in services. (`#1709 <https://github.com/ros2/rclcpp/issues/1709>`_)
+  Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
+* Fix documentation bug. (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_)
+  Signed-off-by: William Woodall <william@osrfoundation.org>
+* Contributors: Ivan Santiago Paunovic, William Woodall
+
+11.1.0 (2021-07-13)
+-------------------
+* Removed left over ``is_initialized()`` implementation (`#1711 <https://github.com/ros2/rclcpp/issues/1711>`_)
+  Leftover from https://github.com/ros2/rclcpp/pull/1622
+* Fixed declare parameter methods for int and float vectors (`#1696 <https://github.com/ros2/rclcpp/issues/1696>`_)
+* Cleaned up implementation of the intra-process manager (`#1695 <https://github.com/ros2/rclcpp/issues/1695>`_)
+* Added the node name to an executor ``runtime_error`` (`#1686 <https://github.com/ros2/rclcpp/issues/1686>`_)
+* Fixed a typo "Attack" -> "Attach" (`#1687 <https://github.com/ros2/rclcpp/issues/1687>`_)
+* Removed use of std::allocator<>::rebind (`#1678 <https://github.com/ros2/rclcpp/issues/1678>`_)
+  rebind is deprecated in c++17 and removed in c++20
+* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Petter Nilsson, Steve Macenski, William Woodall
+
 11.0.0 (2021-05-18)
 -------------------
 * Allow declare uninitialized parameters (`#1673 <https://github.com/ros2/rclcpp/issues/1673>`_)

rclcpp/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.12)
 
 project(rclcpp)
 
@@ -41,7 +41,6 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
-  src/rclcpp/detail/mutex_two_priorities.cpp
   src/rclcpp/detail/resolve_parameter_overrides.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
@@ -110,6 +109,8 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/waitable.cpp
 )
 
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
 # "watch" template for changes
 configure_file(
   "resource/logging.hpp.em"
@@ -123,7 +124,7 @@ set(python_code_logging
 string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
 add_custom_command(OUTPUT include/rclcpp/logging.hpp
   COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
-  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
+  COMMAND Python3::Interpreter ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
@@ -147,7 +148,7 @@ foreach(interface_file ${interface_files})
   string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
   add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
     COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
-    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    COMMAND Python3::Interpreter ARGS -c "${python_${interface_name}_traits}"
     DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
     COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
     VERBATIM
@@ -167,7 +168,7 @@ foreach(interface_file ${interface_files})
   string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
   add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
     COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
-    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    COMMAND Python3::Interpreter ARGS -c "${python_get_${interface_name}}"
     DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
     COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
     VERBATIM
@@ -252,3 +253,5 @@ if(TEST cppcheck)
   # must set the property after ament_package()
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
+
+ament_cmake_gen_version_h()

rclcpp/doc/wait_set_architecture.md

@@ -1,161 +0,0 @@
-# Architecture of the rclcpp::WaitSet Type
-
-The `rclcpp::WaitSet` type is actually a specific configuration of a the more general `rclcpp::WaitSetTemplate` class.
-
-The `rclcpp::WaitSetTemplate` class is design to have interchangeable implementations for controlling storage of items in the wait set, as well as to control access to the wait set functionality, i.e. whether or not it has thread-safety.
-
-In general the idea behind a wait set in rclcpp is that you gather a set of entities that can be waited on, e.g. subscriptions, timers, guard conditions, and then wait for one or more of them to be ready.
-These things are driven by asynchronous events, e.g. subscriptions become ready when new data arrives or timers become ready when their period has elapsed.
-
-At the moment, things that can be waited on are limited by what can be put into an `rcl_wait_set_t`, which includes subscriptions, service clients, service servers, timers, guard conditions, and events.
-In this context events are QoS events, as defined in the `rcl` and `rmw` interfaces.
-
-## Design Goals
-
-There are some design goals for the `rclcpp::WaitSet` family of classes:
-
-- minimize inheritance and avoid virtual functions
-  - Overhead of the executor and wait set design have been a common complaint and are often blamed for performance issues.
-  - Since the wait set is at the core of the wait-do loop of the system, aggressive optimization can actually pay off.
-- avoid unnecessary changes to the underlying `rcl_wait_set_t` instances or unnecessarily recreating it
-  - This can be expensive, because under the hood that requires changes to the middleware's object, e.g. a DDS wait set or similar.
-- minimize memory allocations in the wait() function, avoid them entirely if the set of things to wait on has not changed
-- prevent accidentally using an entity with more than wait set at the same time
-
-Several of these surround performance concerns, but others are concerned with safety and convenience for the user.
-
-## Architecture
-
-The `rclcpp::WaitSetTemplate` is the core fixture in the feature, which is using a policy-based design (see https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design).
-
-It delegates storage and synchronization to policy template arguments to the `rclcpp::WaitSetTemplate` via the `StoragePolicy` and `SynchronizationPolicy` template arguments respectively.
-
-These policies can be mixed and matched to achieve different results.
-There are a few predefined combinations, `rclcpp::WaitSet` which is a non-thread-safe wait set with dynamic storage, `rclcpp::StaticWaitSet` which is a non-thread-safe wait set with fixed storage, and `rclcpp::ThreadSafeWaitSet` which is a thread-safe wait set with dynamic storage.
-They are all just aliases to `rclcpp::WaitSetTemplate` with difference combinations of policies.
-
-Users can extend the behavior by implementing their own policies if desired.
-
-This kind of design allows for specialization and code sharing without using standard polymorphism via virtual functions, and the benefit of that is that calls on the wait set class are always non-virtual, avoiding dispatching via the v-table.
-It also forces us to use templates, which means a head-only implementation, which will increase compile times, but will avoid some calls to shared libraries which on some systems has a small overhead too.
-
-These are aggressive optimizations, but it can actually be very beneficial in the inner-loop of applications, and the hope is that this approach will offer the best performance we can achieve.
-
-### StoragePolicy
-
-The storage policy is responsible for storing the entities in the wait set and providing accessors and setters (if appropriate).
-
-The storage policy is also responsible for maintaining ownership of the entities it is storing at the right times.
-For example, entities are always given to the wait set as `std::shared_ptr`'s and some policies might only maintain ownership for the lifetime of the wait set or until the entity is removed.
-Other policies might only hold ownership of the entities while waiting on them, i.e. while they are actively using them, but maintain only weak ownership otherwise.
-
-There are two built-in storage policies available, the `rclcpp::wait_set_policies::DynamicStorage` and the `rclcpp::wait_set_policies::StaticStorage<...>` classes.
-
-The `StaticStorage` policy requires that you fully specify the number of entities of each time that can be stored as template arguments and uses the `std:array` class as backing storage.
-Therefore, it also does not allow adding or removing entities after construction.
-Because entities cannot be removed and are therefore "always" in use by the wait set, it maintains shared ownership of the entities for the lifetime of the wait set.
-
-The `DynamicStorage` policy provides access to the entities and allows add and removing entities at any time after construction.
-It also only maintains shared ownership of the entities while actively waiting on them, which allows the entities to go out of scope while the wait set is idle.
-Entities that go out of scope in this manner will be removed from the wait set when it notices their weak pointers no longer lock to the entity's shared pointer.
-
-Both policies allow you to initialize the wait set with entities in the constructor.
-
-### SynchronizationPolicy
-
-The synchronization policy is responsible for synchronizing access to wait set operations that share access to the StoragePolicy.
-
-There are two built-in synchronization policies available, the `rclcpp::wait_set_policies::SequentialSynchronization` and the `rclcpp::wait_set_policies::ThreadSafeSynchronization` policies.
-
-The `rclcpp::wait_set_policies::SequentialSynchronization` policy essentially does nothing but pass through requests from the user-facing `rclcpp::WaitSetTemplate` interface to the `StoragePolicy` that is being used.
-
-The `rclcpp::wait_set_policies::ThreadSafeSynchronization` policy sequences access to the `StoragePolicy` being used so that it is safe to call methods like `add_subscription()` on the wait set while it is doing something else, like waiting with the `wait()` method.
-This synchronization comes at the expense of using mutexes and extra logic to organize the access, but provides some thread-safety.
-
-### Entity Intake and Management
-
-Entities, i.e. `rclcpp::SubscriptionBase`, `rclcpp::Waitable`, `rclcpp::ClientBase`, `rclcpp::ServiceBase`, `rclcpp::TimerBase`, `rclcpp::GuardCondition`, etc., may be passed to the wait set during construction.
-If the wait set has policies that allow for dynamic storage and supports adding and removing entities after construction, then entities can also be added and removed using method on the wait set.
-
-Either way, the entities are initially given to the wait set as a `std::shared_ptr` in order to maintain shared ownership, but depending on the storage policy it may be stored as a `std::weak_ptr` while the wait set is not actively using the entities.
-
-Also, when taking on new entities the wait set is responsible for ensuring that entity is not being used by another wait set (at least another instance of `rclcpp::WaitSetTemplate`).
-
-One more thing that plays into intake of entities is that a few entity types have some extra requirements, specifically subscriptions and waitables.
-
-Subscriptions (specifically `rclcpp::SubscriptionBase`) are complex objects that contain not only a subscription that can be waited on, but also may have `rclcpp::Waitable`'s inside in order to implement intra-process communication and for QoS events that are also implemented as `rclcpp::Waitable` instances.
-Therefore, you need a way to say which parts of the subscription should be added to the wait set, because you don't need to add all the entities that make up a subscription to a single wait set.
-The `rclcpp::SubscriptionWaitSetMask` class provides this mapping, and when adding subscriptions to a wait set you need to provide the subscription shared pointer as well as a mask instance.
-
-Waitables are complicated by the fact that they often are part of a larger piece of the API, as we saw with subscriptions, and therefore the waitable may need to keep additional things in scope while being used.
-So when adding a waitable to a wait set you may also provide an "associated entity" as a shared pointer to void, which is purely there to keep something in scope along with the waitable.
-
-#### EntityEntry classes
-
-In order to address the need to pair extra information with some entities (e.g. the subscription mask with the subscription entity) and to provide a convenient place to check if the entity is in use by another wait set and claim if not.
-
-There are a series of classes that are a variation of the `EntityEntry` concept, which wrap the incoming entities and their associated data into a single class, and also provide some safety when converting between the shared ownership and weak ownership if needed.
-
-They also provide RAII-style checking about whether or not an entity is already associated with a wait set.
-This is important because when ingesting multiple entities at the same time, e.g. via the constructor, because if you claim the first few entities to be associated with the wait set, but then find one that is already associated, then you need to abort and throw an exception.
-The RAII-style of these classes addresses this by "dissociating" the first few entities from the wait set when going out of scope due to the thrown exception.
-
-The process from intake to cleanup of the entity entries follows something like this, using waitables as an example:
-
-
-
-                                    ┌───────────────┐
-   ┌────────────────────────┐       │ WaitSet       │
-   │                        ├──────►│               ├───────────────┐
-   │ shared_ptr<Waitable>   │       │ constructor   │               │
-   │                        │       └───────────────┘  Implicit     │
-   │  and optional          │                                       │
-   │                        │       ┌───────────────┐  Conversion   │
-   │ shared_ptr<void>       │       │ add_waitable()│               │
-   │                        ├──────►│               ├───────────────┤
-   └────────────────────────┘       │ method        │               │
-                                    └───────────────┘               │
-                                                                    │
-                                                                    │
-                                                                    │
-          ┌──────────────────────┐        ┌─────────────────┐       │
-          │                      │        │                 │       │
-    ┌─────┤ ManagedWaitableEntry │◄──┬────┤  WaitableEntry  │◄──────┘
-    │     │                      │   │    │                 │
-    │     └─┬──────────────────┬─┘   │    └─┬─────────────┬─┘
-    │       │ shared_ptr+RAII  │     │      │ shared_ptr  │
-    │       └──────────────────┘     │      └─────────────┘
-    │                                │
-    │                        ┌───────┴───────────┐
-    │                        │Check that entity  │
-    │ Conversion to          │is not in use, then│
-    │ weak ownership         │claim it for this  │
-    │ if needed.             │wait set.          │
-    │                        └───────────────────┘
-    │
-    │     ┌───────────────────────────┐
-    │     │                           │
-    └────►│ WeakManagedWaitableEntry  │
-          │                           │
-          └─┬───────────────────────┬─┘
-            │ weak_ptr + RAII       │
-            └───────────────────────┘
-
-Using the diagram as a reference, you can see that the input from the user is converted into the entity entry class that, for now, just stores the entity and any associated data if it exists.
-At this point the entity comes in with shared ownership and stays as shared ownership in the basic entry class.
-Then this entry class is converted into a "managed" entry class, which will check that the entity is not associated with another wait set and associate it with the current wait set during construction.
-This managed entry class will also automatically disassociate the entity with the current wait set during destruction.
-The new managed entry class continues to keep shared ownership of the entity.
-
-Optionally, if the storage policy requires it, the managed entry class can be converted into a "weak" version, where in the entity is stored as a weak pointer instead of a shared pointer.
-In this case the entity will remain stored as a "weak managed" entry until it is removed or the wait set is destroyed.
-In the meantime, if the wait set needs to take shared ownership again, it can do that be getting a copy of the weak pointer(s) in the entry and locking them to get shared pointers.
-But the weak managed entry remains responsible for the RAII-style dissociation from the current wait set when destroyed.
-Unlike the managed entry that maintains shared ownership, the weak version will need to attempt to lock the weak pointer for the entity before trying to dissociate it from the wait set, because this association is maintained within the entity itself, through methods like `rclcpp::Waitable::exchange_in_use_by_wait_set_state()`.
-If the entity cannot be accessed via the weak pointer, then it is not explicitly dissociated by the managed entry, but it doesn't matter because the object has gone out of scope and has been deleted anyway.
-So the cleanup for the weak managed entry is best effort, but that does not pose an issue.
-
-#### The EntityEntryTemplate classes
-
-There is a template class called `rclcpp::wait_set_policies::detail::EntityEntryTemplate<EntityT>` (and associated managed and weak-managed versions) which is the foundation for most of the entity types which don't require special handling, like `rclcpp::GuardCondition` and `rclcpp::TimerBase` for example.
-Other entities like `rclcpp::SubscriptionBase` and `rclcpp::Waitable` have custom specializations of these classes to handle extra storage, custom constructors, and other extra logic to help them work.

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -15,10 +15,12 @@
 #ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 #define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 
+#include <variant>
 #include <functional>
 #include <memory>
 #include <stdexcept>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -29,93 +31,204 @@
 namespace rclcpp
 {
 
+namespace detail
+{
+template<typename T, typename = void>
+struct can_be_nullptr : std::false_type {};
+
+// Some lambdas define a comparison with nullptr,
+// but we see a warning that they can never be null when using it.
+// We also test if `T &` can be assigned to `nullptr` to avoid the issue.
+template<typename T>
+#ifdef __QNXNTO__
+struct can_be_nullptr<T, std::void_t<
+    decltype(std::declval<T>() == nullptr)>>: std::true_type {};
+#else
+struct can_be_nullptr<T, std::void_t<
+    decltype(std::declval<T>() == nullptr), decltype(std::declval<T &>() = nullptr)>>
+  : std::true_type {};
+#endif
+}  // namespace detail
+
+// Forward declare
+template<typename ServiceT>
+class Service;
+
 template<typename ServiceT>
 class AnyServiceCallback
 {
-private:
-  using SharedPtrCallback = std::function<
-    void (
-      const std::shared_ptr<typename ServiceT::Request>,
-      std::shared_ptr<typename ServiceT::Response>
-    )>;
-  using SharedPtrWithRequestHeaderCallback = std::function<
-    void (
-      const std::shared_ptr<rmw_request_id_t>,
-      const std::shared_ptr<typename ServiceT::Request>,
-      std::shared_ptr<typename ServiceT::Response>
-    )>;
-
-  SharedPtrCallback shared_ptr_callback_;
-  SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
-
 public:
   AnyServiceCallback()
-  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
+  : callback_(std::monostate{})
   {}
 
-  AnyServiceCallback(const AnyServiceCallback &) = default;
-
   template<
     typename CallbackT,
-    typename std::enable_if<
+    typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    // Workaround Windows issue with std::bind
+    if constexpr (
       rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
-      >::value
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_callback_ = callback;
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT, can't satisfy both cpplint and uncrustify
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
   }
 
   template<
     typename CallbackT,
-    typename std::enable_if<
+    typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr");
+    }
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT
       rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithRequestHeaderCallback
-      >::value
-    >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_with_request_header_callback_ = callback;
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
   }
 
-  void dispatch(
-    std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<typename ServiceT::Request> request,
-    std::shared_ptr<typename ServiceT::Response> response)
+  // template<typename Allocator = std::allocator<typename ServiceT::Response>>
+  std::shared_ptr<typename ServiceT::Response>
+  dispatch(
+    const std::shared_ptr<rclcpp::Service<ServiceT>> & service_handle,
+    const std::shared_ptr<rmw_request_id_t> & request_header,
+    std::shared_ptr<typename ServiceT::Request> request)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), false);
-    if (shared_ptr_callback_ != nullptr) {
+    if (std::holds_alternative<std::monostate>(callback_)) {
+      // TODO(ivanpauno): Remove the set method, and force the users of this class
+      // to pass a callback at construnciton.
+      throw std::runtime_error{"unexpected request without any callback set"};
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
+      cb(request_header, std::move(request));
+      return nullptr;
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
+      cb(service_handle, request_header, std::move(request));
+      return nullptr;
+    }
+    // auto response = allocate_shared<typename ServiceT::Response, Allocator>();
+    auto response = std::make_shared<typename ServiceT::Response>();
+    if (std::holds_alternative<SharedPtrCallback>(callback_)) {
       (void)request_header;
-      shared_ptr_callback_(request, response);
-    } else if (shared_ptr_with_request_header_callback_ != nullptr) {
-      shared_ptr_with_request_header_callback_(request_header, request, response);
-    } else {
-      throw std::runtime_error("unexpected request without any callback set");
+      const auto & cb = std::get<SharedPtrCallback>(callback_);
+      cb(std::move(request), response);
+    } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
+      cb(request_header, std::move(request), response);
     }
     TRACEPOINT(callback_end, static_cast<const void *>(this));
+    return response;
   }
 
   void register_callback_for_tracing()
   {
 #ifndef TRACETOOLS_DISABLED
-    if (shared_ptr_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        static_cast<const void *>(this),
-        tracetools::get_symbol(shared_ptr_callback_));
-    } else if (shared_ptr_with_request_header_callback_) {
-      TRACEPOINT(
-        rclcpp_callback_register,
-        static_cast<const void *>(this),
-        tracetools::get_symbol(shared_ptr_with_request_header_callback_));
-    }
+    std::visit(
+      [this](auto && arg) {
+        TRACEPOINT(
+          rclcpp_callback_register,
+          static_cast<const void *>(this),
+          tracetools::get_symbol(arg));
+      }, callback_);
 #endif  // TRACETOOLS_DISABLED
   }
+
+private:
+  using SharedPtrCallback = std::function<
+    void (
+      std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrDeferResponseCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>
+    )>;
+  using SharedPtrDeferResponseCallbackWithServiceHandle = std::function<
+    void (
+      std::shared_ptr<rclcpp::Service<ServiceT>>,
+      std::shared_ptr<rmw_request_id_t>,
+      std::shared_ptr<typename ServiceT::Request>
+    )>;
+
+  std::variant<
+    std::monostate,
+    SharedPtrCallback,
+    SharedPtrWithRequestHeaderCallback,
+    SharedPtrDeferResponseCallback,
+    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -406,10 +406,10 @@ public:
 
   /// Function for shared_ptr to non-const MessageT, which is deprecated.
   template<typename SetT>
-  // TODO(wjwwood): enable this deprecation after Galactic
-  // [[deprecated(
-  //   "use 'void (std::shared_ptr<const MessageT>)' instead"
-  // )]]
+#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
+  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
+  [[deprecated("use 'void(std::shared_ptr<const MessageT>)' instead")]]
+#endif
   void
   set_deprecated(std::function<void(std::shared_ptr<SetT>)> callback)
   {
@@ -418,10 +418,12 @@ public:
 
   /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated.
   template<typename SetT>
-  // TODO(wjwwood): enable this deprecation after Galactic
-  // [[deprecated(
-  //   "use 'void (std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
-  // )]]
+#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
+  // suppress deprecation warnings in `test_any_subscription_callback.cpp`
+  [[deprecated(
+          "use 'void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
+  )]]
+#endif
   void
   set_deprecated(std::function<void(std::shared_ptr<SetT>, const rclcpp::MessageInfo &)> callback)
   {
@@ -474,7 +476,22 @@ public:
     }
   }
 
-  // Dispatch when input is a ros message and the output could be anything.
+  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
+  convert_custom_type_message_to_ros_message_unique_ptr(const SubscribedType & msg)
+  {
+    if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+      auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
+      ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr);
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ptr);
+      return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
+    } else {
+      throw std::runtime_error(
+              "convert_custom_type_message_to_ros_message_unique_ptr "
+              "unexpectedly called without TypeAdapter");
+    }
+  }
+
+  // Dispatch when input is a ROS message and the output could be anything.
   void
   dispatch(
     std::shared_ptr<ROSMessageType> message,
@@ -527,7 +544,7 @@ public:
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         }
-        // conditions for output is ros message
+        // conditions for output is ROS message
         else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
           callback(*message);
         } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
@@ -654,7 +671,12 @@ public:
     TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
-  void
+  template<typename T>
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<T>::value &&
+    std::is_same<T, ROSMessageType>::value
+  >
   dispatch_intra_process(
     std::shared_ptr<const ROSMessageType> message,
     const rclcpp::MessageInfo & message_info)
@@ -670,83 +692,83 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && callback) {
-        using T = std::decay_t<decltype(callback)>;
+        using CallbackT = std::decay_t<decltype(callback)>;
         static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
 
         // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
+        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
           auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
           callback(*local_message);
-        } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
+        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
           auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
           callback(*local_message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, UniquePtrCallback>||
-            std::is_same_v<T, SharedPtrCallback>
+            std::is_same_v<CallbackT, UniquePtrCallback>||
+            std::is_same_v<CallbackT, SharedPtrCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, UniquePtrWithInfoCallback>||
-            std::is_same_v<T, SharedPtrWithInfoCallback>
+            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
+            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, SharedConstPtrCallback>||
-            std::is_same_v<T, ConstRefSharedConstPtrCallback>
+            std::is_same_v<CallbackT, SharedConstPtrCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
+            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         }
-        // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
+        // conditions for ROS message type
+        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
           callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
+        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
           callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, UniquePtrROSMessageCallback>||
-          std::is_same_v<T, SharedPtrROSMessageCallback>)
+          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
         {
           callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
+          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
         {
           callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
+          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
         {
           callback(message);
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
           callback(message, message_info);
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, ConstRefSerializedMessageCallback>||
-          std::is_same_v<T, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<T, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<T, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<T, SharedPtrSerializedMessageWithInfoCallback>)
+          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
         {
           throw std::runtime_error(
             "Cannot dispatch std::shared_ptr<const ROSMessageType> message "
@@ -754,13 +776,18 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(always_false_v<CallbackT>, "unhandled callback type");
         }
       }, callback_variant_);
     TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
-  void
+  template<typename T>
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<T>::value &&
+    std::is_same<T, ROSMessageType>::value
+  >
   dispatch_intra_process(
     std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message,
     const rclcpp::MessageInfo & message_info)
@@ -776,83 +803,83 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && callback) {
-        using T = std::decay_t<decltype(callback)>;
+        using CallbackT = std::decay_t<decltype(callback)>;
         static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
 
         // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
+        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
           auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
           callback(*local_message);
-        } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
+        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
           auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
           callback(*local_message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, UniquePtrCallback>||
-            std::is_same_v<T, SharedPtrCallback>
+            std::is_same_v<CallbackT, UniquePtrCallback>||
+            std::is_same_v<CallbackT, SharedPtrCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, UniquePtrWithInfoCallback>||
-            std::is_same_v<T, SharedPtrWithInfoCallback>
+            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
+            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, SharedConstPtrCallback>||
-            std::is_same_v<T, ConstRefSharedConstPtrCallback>
+            std::is_same_v<CallbackT, SharedConstPtrCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<T, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
+            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         }
-        // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
+        // conditions for ROS message type
+        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
           callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
+        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
           callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, UniquePtrROSMessageCallback>||
-          std::is_same_v<T, SharedPtrROSMessageCallback>)
+          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
         {
           callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
+          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
         {
           callback(std::move(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
+          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
         {
           callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
           callback(std::move(message), message_info);
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<T, ConstRefSerializedMessageCallback>||
-          std::is_same_v<T, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<T, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<T, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<T, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<T, SharedPtrSerializedMessageWithInfoCallback>)
+          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
         {
           throw std::runtime_error(
             "Cannot dispatch std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message "
@@ -860,7 +887,227 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<T>, "unhandled callback type");
+          static_assert(always_false_v<CallbackT>, "unhandled callback type");
+        }
+      }, callback_variant_);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
+  }
+
+  template<typename T>
+  typename std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, SubscribedType>::value
+  >
+  dispatch_intra_process(
+    std::shared_ptr<const T> message,
+    const rclcpp::MessageInfo & message_info)
+  {
+    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    // Check if the variant is "unset", throw if it is.
+    if (callback_variant_.index() == 0) {
+      if (std::get<0>(callback_variant_) == nullptr) {
+        // This can happen if it is default initialized, or if it is assigned nullptr.
+        throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback");
+      }
+    }
+    // Dispatch.
+    std::visit(
+      [&message, &message_info, this](auto && callback) {
+        using CallbackT = std::decay_t<decltype(callback)>;
+        static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
+
+        // conditions for custom type
+        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
+          callback(*message);
+        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
+          callback(*message);
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, UniquePtrCallback>||
+            std::is_same_v<CallbackT, SharedPtrCallback>
+        ))
+        {
+          callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
+            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
+        ))
+        {
+          callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, SharedConstPtrCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
+        ))
+        {
+          callback(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(message.get(), subscribed_type_deleter_));
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
+        ))
+        {
+          callback(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(message.get(), subscribed_type_deleter_), message_info);
+        }
+        // conditions for ROS message type
+        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
+          auto local_message = convert_custom_type_message_to_ros_message_unique_ptr(*message);
+          callback(*local_message);
+        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
+          auto local_message = convert_custom_type_message_to_ros_message_unique_ptr(*message);
+          callback(*local_message, message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
+        {
+          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
+        {
+          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message), message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
+        {
+          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+        {
+          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message), message_info);
+        }
+        // condition to catch SerializedMessage types
+        else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
+        {
+          throw std::runtime_error(
+            "Cannot dispatch std::shared_ptr<const ROSMessageType> message "
+            "to rclcpp::SerializedMessage");
+        }
+        // condition to catch unhandled callback types
+        else {  // NOLINT[readability/braces]
+          static_assert(always_false_v<CallbackT>, "unhandled callback type");
+        }
+      }, callback_variant_);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
+  }
+
+  template<typename T>
+  typename std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, SubscribedType>::value
+  >
+  dispatch_intra_process(
+    std::unique_ptr<T, SubscribedTypeDeleter> message,
+    const rclcpp::MessageInfo & message_info)
+  {
+    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
+    // Check if the variant is "unset", throw if it is.
+    if (callback_variant_.index() == 0) {
+      if (std::get<0>(callback_variant_) == nullptr) {
+        // This can happen if it is default initialized, or if it is assigned nullptr.
+        throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback");
+      }
+    }
+    // Dispatch.
+    std::visit(
+      [&message, &message_info, this](auto && callback) {
+        using CallbackT = std::decay_t<decltype(callback)>;
+        static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
+
+        // conditions for custom type
+        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
+          callback(*message);
+        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
+          callback(*message);
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, UniquePtrCallback>||
+            std::is_same_v<CallbackT, SharedPtrCallback>
+        ))
+        {
+          callback(std::move(message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
+            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
+        ))
+        {
+          callback(std::move(message), message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, SharedConstPtrCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
+        ))
+        {
+          callback(std::move(message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          is_ta && (
+            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
+        ))
+        {
+          callback(std::move(message), message_info);
+        }
+        // conditions for ROS message type
+        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
+          auto local_message = convert_custom_type_to_ros_message_unique_ptr(*message);
+          callback(*local_message);
+        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
+          auto local_message = convert_custom_type_to_ros_message_unique_ptr(*message);
+          callback(*local_message, message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
+        {
+          callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
+        {
+          callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
+        {
+          callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+        } else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+        {
+          callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+        }
+        // condition to catch SerializedMessage types
+        else if constexpr (  // NOLINT[readability/braces]
+          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
+        {
+          throw std::runtime_error(
+            "Cannot dispatch std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message "
+            "to rclcpp::SerializedMessage");
+        }
+        // condition to catch unhandled callback types
+        else {  // NOLINT[readability/braces]
+          static_assert(always_false_v<CallbackT>, "unhandled callback type");
         }
       }, callback_variant_);
     TRACEPOINT(callback_end, static_cast<const void *>(this));

rclcpp/include/rclcpp/client.hpp

@@ -17,12 +17,15 @@
 
 #include <atomic>
 #include <future>
-#include <map>
+#include <unordered_map>
 #include <memory>
+#include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>  // NOLINT
+#include <vector>
 
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
@@ -45,6 +48,68 @@
 namespace rclcpp
 {
 
+namespace detail
+{
+template<typename FutureT>
+struct FutureAndRequestId
+{
+  FutureT future;
+  int64_t request_id;
+
+  FutureAndRequestId(FutureT impl, int64_t req_id)
+  : future(std::move(impl)), request_id(req_id)
+  {}
+
+  /// Allow implicit conversions to `std::future` by reference.
+  operator FutureT &() {return this->future;}
+
+  /// Deprecated, use the `future` member variable instead.
+  /**
+   * Allow implicit conversions to `std::future` by value.
+   * \deprecated
+   */
+  [[deprecated("FutureAndRequestId: use .future instead of an implicit conversion")]]
+  operator FutureT() {return this->future;}
+
+  // delegate future like methods in the std::future impl_
+
+  /// See std::future::get().
+  auto get() {return this->future.get();}
+  /// See std::future::valid().
+  bool valid() const noexcept {return this->future.valid();}
+  /// See std::future::wait().
+  void wait() const {return this->future.wait();}
+  /// See std::future::wait_for().
+  template<class Rep, class Period>
+  std::future_status wait_for(
+    const std::chrono::duration<Rep, Period> & timeout_duration) const
+  {
+    return this->future.wait_for(timeout_duration);
+  }
+  /// See std::future::wait_until().
+  template<class Clock, class Duration>
+  std::future_status wait_until(
+    const std::chrono::time_point<Clock, Duration> & timeout_time) const
+  {
+    return this->future.wait_until(timeout_time);
+  }
+
+  // Rule of five, we could use the rule of zero here, but better be explicit as some of the
+  // methods are deleted.
+
+  /// Move constructor.
+  FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy constructor, each instance is a unique owner of the future.
+  FutureAndRequestId(const FutureAndRequestId & other) = delete;
+  /// Move assignment.
+  FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
+  /// Deleted copy assignment, each instance is a unique owner of the future.
+  FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
+  /// Destructor.
+  ~FutureAndRequestId() = default;
+};
+}  // namespace detail
+
 namespace node_interfaces
 {
 class NodeBaseInterface;
@@ -178,6 +243,9 @@ template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
+  using Request = typename ServiceT::Request;
+  using Response = typename ServiceT::Response;
+
   using SharedRequest = typename ServiceT::Request::SharedPtr;
   using SharedResponse = typename ServiceT::Response::SharedPtr;
 
@@ -187,6 +255,7 @@ public:
   using SharedPromise = std::shared_ptr<Promise>;
   using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
 
+  using Future = std::future<SharedResponse>;
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
@@ -195,6 +264,64 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
+  /// A convenient Client::Future and request id pair.
+  /**
+   * Public members:
+   * - future: a std::future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::future provides.
+   */
+  struct FutureAndRequestId
+    : detail::FutureAndRequestId<std::future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::future<SharedResponse>>::FutureAndRequestId;
+
+    /// Deprecated, use `.future.share()` instead.
+    /**
+     * Allow implicit conversions to `std::shared_future` by value.
+     * \deprecated
+     */
+    [[deprecated(
+      "FutureAndRequestId: use .future.share() instead of an implicit conversion")]]
+    operator SharedFuture() {return this->future.share();}
+
+    // delegate future like methods in the std::future impl_
+
+    /// See std::future::share().
+    SharedFuture share() noexcept {return this->future.share();}
+  };
+
+  /// A convenient Client::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
+  /// A convenient Client::SharedFutureWithRequest and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureWithRequestAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<std::pair<SharedRequest, SharedResponse>>>
+  {
+    using detail::FutureAndRequestId<
+      std::shared_future<std::pair<SharedRequest, SharedResponse>>
+    >::FutureAndRequestId;
+  };
+
   /// Default constructor.
   /**
    * The constructor for a Client is almost never called directly.
@@ -292,34 +419,89 @@ public:
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response) override
   {
-    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
-    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = request_header->sequence_number;
-    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
-    if (this->pending_requests_.count(sequence_number) == 0) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Received invalid sequence number. Ignoring...");
+    std::optional<CallbackInfoVariant>
+    optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number);
+    if (!optional_pending_request) {
       return;
     }
-    auto tuple = this->pending_requests_[sequence_number];
-    auto call_promise = std::get<0>(tuple);
-    auto callback = std::get<1>(tuple);
-    auto future = std::get<2>(tuple);
-    this->pending_requests_.erase(sequence_number);
-    // Unlock here to allow the service to be called recursively from one of its callbacks.
-    lock.unlock();
-
-    call_promise->set_value(typed_response);
-    callback(future);
+    auto & value = *optional_pending_request;
+    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(
+      std::move(response));
+    if (std::holds_alternative<Promise>(value)) {
+      auto & promise = std::get<Promise>(value);
+      promise.set_value(std::move(typed_response));
+    } else if (std::holds_alternative<CallbackTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackType>(inner);
+      auto & promise = std::get<Promise>(inner);
+      auto & future = std::get<SharedFuture>(inner);
+      promise.set_value(std::move(typed_response));
+      callback(std::move(future));
+    } else if (std::holds_alternative<CallbackWithRequestTypeValueVariant>(value)) {
+      auto & inner = std::get<CallbackWithRequestTypeValueVariant>(value);
+      const auto & callback = std::get<CallbackWithRequestType>(inner);
+      auto & promise = std::get<PromiseWithRequest>(inner);
+      auto & future = std::get<SharedFutureWithRequest>(inner);
+      auto & request = std::get<SharedRequest>(inner);
+      promise.set_value(std::make_pair(std::move(request), std::move(typed_response)));
+      callback(std::move(future));
+    }
   }
 
-  SharedFuture
+  /// Send a request to the service server.
+  /**
+   * This method returns a `FutureAndRequestId` instance
+   * that can be passed to Executor::spin_until_future_complete() to
+   * wait until it has been completed.
+   *
+   * If the future never completes,
+   * e.g. the call to Executor::spin_until_future_complete() times out,
+   * Client::remove_pending_request() must be called to clean the client internal state.
+   * Not doing so will make the `Client` instance to use more memory each time a response is not
+   * received from the service server.
+   *
+   * ```cpp
+   * auto future = client->async_send_request(my_request);
+   * if (
+   *   rclcpp::FutureReturnCode::TIMEOUT ==
+   *   executor->spin_until_future_complete(future, timeout))
+   * {
+   *   client->remove_pending_request(future);
+   *   // handle timeout
+   * } else {
+   *   handle_response(future.get());
+   * }
+   * ```
+   *
+   * \param[in] request request to be send.
+   * \return a FutureAndRequestId instance.
+   */
+  FutureAndRequestId
   async_send_request(SharedRequest request)
   {
-    return async_send_request(request, [](SharedFuture) {});
+    Promise promise;
+    auto future = promise.get_future();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::move(promise));
+    return FutureAndRequestId(std::move(future), req_id);
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server, remove_pending_request()
+   * has to be called with the returned request id or prune_pending_requests().
+   * Not doing so will make the `Client` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -329,23 +511,28 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFuture
+  SharedFutureAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-    int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
-    }
-
-    SharedPromise call_promise = std::make_shared<Promise>();
-    SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] =
-      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
-    return f;
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
   }
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous method, but you can get both the request and response in the callback.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
   template<
     typename CallbackT,
     typename std::enable_if<
@@ -355,28 +542,165 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureWithRequest
+  SharedFutureWithRequestAndRequestId
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
-    SharedFutureWithRequest future_with_request(promise->get_future());
-
-    auto wrapping_cb = [future_with_request, promise, request,
-        cb = std::forward<CallbackWithRequestType>(cb)](SharedFuture future) {
-        auto response = future.get();
-        promise->set_value(std::make_pair(request, response));
-        cb(future_with_request);
-      };
-
-    async_send_request(request, wrapping_cb);
-
-    return future_with_request;
+    PromiseWithRequest promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      *request,
+      std::make_tuple(
+        CallbackWithRequestType{std::forward<CallbackT>(cb)},
+        request,
+        shared_future,
+        std::move(promise)));
+    return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id};
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
+  bool
+  remove_pending_request(int64_t request_id)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    return pending_requests_.erase(request_id) != 0u;
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const FutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `Client::remove_pending_request(this, future.request_id)`.
+   */
+  bool
+  remove_pending_request(const SharedFutureWithRequestAndRequestId & future)
+  {
+    return this->remove_pending_request(future.request_id);
+  }
+
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
+  size_t
+  prune_pending_requests()
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    auto ret = pending_requests_.size();
+    pending_requests_.clear();
+    return ret;
+  }
+
+  /// Clean all pending requests older than a time_point.
+  /**
+   * \param[in] time_point Requests that were sent before this point are going to be removed.
+   * \param[inout] pruned_requests Removed requests id will be pushed to the vector
+   *  if a pointer is provided.
+   * \return number of pending requests that were removed.
+   */
+  template<typename AllocatorT = std::allocator<int64_t>>
+  size_t
+  prune_requests_older_than(
+    std::chrono::time_point<std::chrono::system_clock> time_point,
+    std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    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);
+        it = pending_requests_.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    return old_size - pending_requests_.size();
+  }
+
+protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
+  using CallbackWithRequestTypeValueVariant = std::tuple<
+    CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>;
+
+  using CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant,
+    CallbackWithRequestTypeValueVariant>;
+
+  int64_t
+  async_send_request_impl(const Request & request, CallbackInfoVariant value)
+  {
+    int64_t sequence_number;
+    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)));
+    }
+    return sequence_number;
+  }
+
+  std::optional<CallbackInfoVariant>
+  get_and_erase_pending_request(int64_t request_number)
+  {
+    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
+    auto it = this->pending_requests_.find(request_number);
+    if (it == this->pending_requests_.end()) {
+      RCUTILS_LOG_DEBUG_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
+      return std::nullopt;
+    }
+    auto value = std::move(it->second.second);
+    this->pending_requests_.erase(request_number);
+    return value;
   }
 
-private:
   RCLCPP_DISABLE_COPY(Client)
 
-  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::unordered_map<
+    int64_t,
+    std::pair<
+      std::chrono::time_point<std::chrono::system_clock>,
+      CallbackInfoVariant>>
+  pending_requests_;
   std::mutex pending_requests_mutex_;
 };
 

rclcpp/include/rclcpp/clock.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <mutex>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -78,6 +79,30 @@ public:
   Time
   now();
 
+  /**
+   * Sleep until a specified Time, according to clock type.
+   *
+   * Notes for RCL_ROS_TIME clock type:
+   *   - Can sleep forever if ros time is active and received clock never reaches `until`
+   *   - If ROS time enabled state changes during the sleep, this method will immediately return
+   *     false. There is not a consistent choice of sleeping time when the time source changes,
+   *     so this is up to the caller to call again if needed.
+   *
+   * \param until absolute time according to current clock type to sleep until.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true immediately if `until` is in the past
+   * \return true when the time `until` is reached
+   * \return false if time cannot be reached reliably, for example from shutdown or a change
+   *    of time source.
+   * \throws std::runtime_error if the context is invalid
+   * \throws std::runtime_error if `until` has a different clock type from this clock
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_until(
+    Time until,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/context.hpp

@@ -48,17 +48,20 @@ public:
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;
 
-class OnShutdownCallbackHandle
+class ShutdownCallbackHandle
 {
   friend class Context;
 
 public:
-  using OnShutdownCallbackType = std::function<void ()>;
+  using ShutdownCallbackType = std::function<void ()>;
 
 private:
-  std::weak_ptr<OnShutdownCallbackType> callback;
+  std::weak_ptr<ShutdownCallbackType> callback;
 };
 
+using OnShutdownCallbackHandle = ShutdownCallbackHandle;
+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.
@@ -75,7 +78,7 @@ public:
    * Every context which is constructed is added to a global vector of contexts,
    * which is used by the signal handler to conditionally shutdown each context
    * on SIGINT.
-   * See the shutdown_on_sigint option in the InitOptions class.
+   * See the shutdown_on_signal option in the InitOptions class.
    */
   RCLCPP_PUBLIC
   Context();
@@ -93,7 +96,7 @@ public:
    * Note that this function does not setup any signal handlers, so if you want
    * it to be shutdown by the signal handler, then you need to either install
    * them manually with rclcpp::install_signal_handlers() or use rclcpp::init().
-   * In addition to installing the signal handlers, the shutdown_on_sigint
+   * In addition to installing the signal handlers, the shutdown_on_signal
    * of the InitOptions needs to be `true` for this context to be shutdown by
    * the signal handler, otherwise it will be passed over.
    *
@@ -189,7 +192,7 @@ public:
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;
+  using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -197,7 +200,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronoulsy in the dedicated singal handling thread.
+   * asynchronously in the dedicated singal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -221,7 +224,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronously in the dedicated singal handling thread.
+   * asynchronously in the dedicated signal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -249,6 +252,33 @@ public:
   bool
   remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
 
+  using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
+
+  /// Add a pre_shutdown callback to be called before shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added.
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated signal handling thread.
+   *
+   * \param[in] callback the pre_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreShutdownCallbackHandle
+  add_pre_shutdown_callback(PreShutdownCallback callback);
+
+  /// Remove an registered pre_shutdown callback.
+  /**
+   * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
+
   /// Return the shutdown callbacks.
   /**
    * Returned callbacks are a copy of the registered callbacks.
@@ -257,6 +287,14 @@ public:
   std::vector<OnShutdownCallback>
   get_on_shutdown_callbacks() const;
 
+  /// Return the pre-shutdown callbacks.
+  /**
+   * Returned callbacks are a copy of the registered callbacks.
+   */
+  RCLCPP_PUBLIC
+  std::vector<PreShutdownCallback>
+  get_pre_shutdown_callbacks() const;
+
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_context_t>
@@ -268,7 +306,7 @@ public:
    *
    *   - this context is shutdown()
    *   - this context is destructed (resulting in shutdown)
-   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown)
    *   - interrupt_all_sleep_for() is called
    *
    * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
@@ -338,6 +376,9 @@ private:
   std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
+  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::mutex pre_shutdown_callbacks_mutex_;
+
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;
   /// Mutex for protecting the global condition variable.
@@ -345,6 +386,29 @@ private:
 
   /// Keep shared ownership of global vector of weak contexts
   std::shared_ptr<WeakContextsWrapper> weak_contexts_;
+
+  enum class ShutdownType
+  {
+    pre_shutdown,
+    on_shutdown
+  };
+
+  using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
+
+  RCLCPP_LOCAL
+  ShutdownCallbackHandle
+  add_shutdown_callback(
+    ShutdownType shutdown_type,
+    ShutdownCallback callback);
+
+  RCLCPP_LOCAL
+  bool
+  remove_shutdown_callback(
+    ShutdownType shutdown_type,
+    const ShutdownCallbackHandle & callback_handle);
+
+  std::vector<rclcpp::Context::ShutdownCallback>
+  get_shutdown_callback(ShutdownType shutdown_type) const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

@@ -1,76 +0,0 @@
-// Copyright 2021 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__MUTEX_TWO_PRIORITIES_HPP_
-#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
-
-#include <condition_variable>
-#include <mutex>
-
-namespace rclcpp
-{
-namespace detail
-{
-/// \internal A mutex that has two locking mechanism, one with higher priority than the other.
-/**
- * After the current mutex owner release the lock, a thread that used the high
- * priority mechanism will have priority over threads that used the low priority mechanism.
- */
-class MutexTwoPriorities
-{
-public:
-  class HighPriorityLockable
-  {
-public:
-    explicit HighPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  class LowPriorityLockable
-  {
-public:
-    explicit LowPriorityLockable(MutexTwoPriorities & parent);
-
-    void lock();
-
-    void unlock();
-
-private:
-    MutexTwoPriorities & parent_;
-  };
-
-  HighPriorityLockable
-  get_high_priority_lockable();
-
-  LowPriorityLockable
-  get_low_priority_lockable();
-
-private:
-  std::condition_variable hp_cv_;
-  std::condition_variable lp_cv_;
-  std::mutex cv_mutex_;
-  size_t hp_waiting_count_{0u};
-  bool data_taken_{false};
-};
-
-}  // namespace detail
-}  // namespace rclcpp
-
-#endif  // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -29,6 +29,7 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -40,7 +41,6 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/scope_exit.hpp"
 
 namespace rclcpp
 {
@@ -354,7 +354,7 @@ public:
     if (spinning.exchange(true)) {
       throw std::runtime_error("spin_until_future_complete() called while already spinning");
     }
-    RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
     while (rclcpp::ok(this->context_) && spinning.load()) {
       // Do one item of work.
       spin_once_impl(timeout_left);

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -22,7 +22,6 @@
 #include <thread>
 #include <unordered_map>
 
-#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -82,12 +81,10 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  detail::MutexTwoPriorities wait_mutex_;
+  std::mutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;
-
-  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -57,15 +57,13 @@ public:
   /**
    * \param p_wait_set A reference to the wait set to be used in the executor
    * \param memory_strategy Shared pointer to the memory strategy to set.
-   * \param executor_guard_condition executor's guard condition
    * \throws std::runtime_error if memory strategy is null
    */
   RCLCPP_PUBLIC
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
-    rcl_guard_condition_t * executor_guard_condition);
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
   /// Finalize StaticExecutorEntitiesCollector to clear resources
   RCLCPP_PUBLIC
@@ -338,6 +336,10 @@ private:
   /// List of weak nodes registered in the static executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 
+  // Mutex to protect vector of new nodes.
+  std::mutex new_nodes_mutex_;
+  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
+
   /// Wait set for managing entities that the rmw layer waits on.
   rcl_wait_set_t * p_wait_set_ = nullptr;
 

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

@@ -16,6 +16,7 @@
 #define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 #include <utility>
 

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

@@ -15,10 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 
-#include <algorithm>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
 #include <mutex>
 #include <stdexcept>
 #include <utility>

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -16,11 +16,9 @@
 #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
-#include <type_traits>
+#include <stdexcept>
 #include <utility>
 
-#include "rcl/subscription.h"
-
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
 #include "rclcpp/intra_process_buffer_type.hpp"

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 #define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
 
-#include <memory>
 #include <vector>
 
 #include "rclcpp/client.hpp"

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -19,17 +19,16 @@
 
 #include <shared_mutex>
 
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
+#include <iterator>
 #include <memory>
-#include <string>
+#include <stdexcept>
+#include <type_traits>
 #include <unordered_map>
 #include <utility>
 #include <vector>
 
+#include "rosidl_runtime_cpp/traits.hpp"
+
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/experimental/subscription_intra_process.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
@@ -38,6 +37,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/publisher_base.hpp"
+#include "rclcpp/type_adapter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -175,17 +175,32 @@ public:
    * \param message the message that is being stored.
    */
   template<
+    typename T,
+    typename DeleterT,
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
-  void
+    typename AllocatorT = std::allocator<void>,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>,
+    typename PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>,
+    typename PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type
+  >
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<T>::value &&
+    std::is_same<T, ROSMessageType>::value
+  >
   do_intra_process_publish(
     uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT, Deleter> message,
-    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+    std::unique_ptr<T, DeleterT> message,
+    ROSMessageTypeAllocator & ros_message_type_allocator,
+    ROSMessageTypeDeleter & ros_message_type_deleter,
+    PublishedTypeAllocator & published_type_allocator)
   {
-    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
-    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+    (void)ros_message_type_deleter;
+    (void)published_type_allocator;
 
     std::shared_lock<std::shared_timed_mutex> lock(mutex_);
 
@@ -201,53 +216,152 @@ public:
 
     if (sub_ids.take_ownership_subscriptions.empty()) {
       // None of the buffers require ownership, so we promote the pointer
-      std::shared_ptr<MessageT> msg = std::move(message);
+      std::shared_ptr<ROSMessageType> msg = std::move(message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_shared_msg_to_buffers<ROSMessageType, AllocatorT>(
         msg, sub_ids.take_shared_subscriptions);
-    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
-      sub_ids.take_shared_subscriptions.size() <= 1)
-    {
-      // There is at maximum 1 buffer that does not require ownership.
-      // So this case is equivalent to all the buffers requiring ownership
+    } else {
+      if (sub_ids.take_shared_subscriptions.size() <= 1) {
+        // There is at maximum 1 buffer that does not require ownership.
+        // So this case is equivalent to all the buffers requiring ownership
 
-      // Merge the two vector of ids into a unique one
-      std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);
-      concatenated_vector.insert(
-        concatenated_vector.end(),
-        sub_ids.take_ownership_subscriptions.begin(),
-        sub_ids.take_ownership_subscriptions.end());
+        // Merge the two vector of ids into a unique one
+        std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);
+        concatenated_vector.insert(
+          concatenated_vector.end(),
+          sub_ids.take_ownership_subscriptions.begin(),
+          sub_ids.take_ownership_subscriptions.end());
 
-      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
-        std::move(message),
-        concatenated_vector,
-        allocator);
-    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
-      sub_ids.take_shared_subscriptions.size() > 1)
-    {
-      // Construct a new shared pointer from the message
-      // for the buffers that do not require ownership
-      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
+        this->template add_owned_msg_to_buffers<ROSMessageType, AllocatorT>(
+          std::move(message),
+          concatenated_vector,
+          ros_message_type_allocator);
+      } else {
+        // Construct a new shared pointer from the message
+        // for the buffers that do not require ownership
+        auto shared_msg = std::allocate_shared<ROSMessageType, ROSMessageTypeAllocator>(
+          ros_message_type_allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
-        shared_msg, sub_ids.take_shared_subscriptions);
-      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
-        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
+        this->template add_shared_msg_to_buffers<ROSMessageType, AllocatorT>(
+          shared_msg, sub_ids.take_shared_subscriptions);
+        this->template add_owned_msg_to_buffers<ROSMessageType, AllocatorT>(
+          std::move(message), sub_ids.take_ownership_subscriptions, ros_message_type_allocator);
+      }
     }
   }
 
   template<
+    typename T,
+    typename DeleterT,
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
-  std::shared_ptr<const MessageT>
+    typename AllocatorT = std::allocator<void>,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>,
+    typename PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>,
+    typename PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type
+  >
+  typename
+  std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, PublishedType>::value
+  >
+  do_intra_process_publish(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<T, DeleterT> message,
+    ROSMessageTypeAllocator & ros_message_type_allocator,
+    ROSMessageTypeDeleter & ros_message_type_deleter,
+    PublishedTypeAllocator & published_type_allocator)
+  {
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // None of the buffers require ownership, so we promote the pointer
+      std::shared_ptr<T> msg = std::move(message);
+
+      this->template add_blah_shared_msg_to_buffers<MessageT, T, AllocatorT>(
+        msg,
+        sub_ids.take_shared_subscriptions,
+        ros_message_type_allocator,
+        ros_message_type_deleter);
+    } else {
+      if (sub_ids.take_shared_subscriptions.size() <= 1) {
+        // There is at maximum 1 buffer that does not require ownership.
+        // So this case is equivalent to all the buffers requiring ownership
+
+        // Merge the two vector of ids into a unique one
+        std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);
+        concatenated_vector.insert(
+          concatenated_vector.end(),
+          sub_ids.take_ownership_subscriptions.begin(),
+          sub_ids.take_ownership_subscriptions.end());
+
+        this->template add_blah_owned_msg_to_buffers<MessageT, T, AllocatorT>(
+          std::move(message),
+          concatenated_vector,
+          ros_message_type_allocator,
+          ros_message_type_deleter);
+      } else {
+        // Construct a new shared pointer from the message
+        // for the buffers that do not require ownership
+        auto shared_msg = std::allocate_shared<T, PublishedTypeAllocator>(
+          published_type_allocator,
+          *message);
+
+        this->template add_blah_shared_msg_to_buffers<MessageT, T, AllocatorT>(
+          shared_msg,
+          sub_ids.take_shared_subscriptions,
+          ros_message_type_allocator,
+          ros_message_type_deleter);
+        this->template add_blah_owned_msg_to_buffers<MessageT, T, AllocatorT>(
+          std::move(message),
+          sub_ids.take_ownership_subscriptions,
+          ros_message_type_allocator,
+          ros_message_type_deleter);
+      }
+    }
+  }
+
+  template<
+    typename T,
+    typename DeleterT,
+    typename MessageT,
+    typename AllocatorT,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>,
+    typename PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>,
+    typename PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type
+  >
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<T>::value &&
+    std::is_same<T, ROSMessageType>::value,
+    std::shared_ptr<const ROSMessageType>
+  >
   do_intra_process_publish_and_return_shared(
     uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT, Deleter> message,
-    typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
+    std::unique_ptr<T, DeleterT> message,
+    ROSMessageTypeAllocator & ros_message_type_allocator,
+    ROSMessageTypeDeleter & ros_message_type_deleter,
+    PublishedTypeAllocator & published_type_allocator)
   {
-    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
-    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+    (void)ros_message_type_deleter;
+    (void)published_type_allocator;
 
     std::shared_lock<std::shared_timed_mutex> lock(mutex_);
 
@@ -263,33 +377,111 @@ public:
 
     if (sub_ids.take_ownership_subscriptions.empty()) {
       // If there are no owning, just convert to shared.
-      std::shared_ptr<MessageT> shared_msg = std::move(message);
+      std::shared_ptr<ROSMessageType> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<ROSMessageType, AllocatorT>(
           shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
     } else {
       // Construct a new shared pointer from the message for the buffers that
       // do not require ownership and to return.
-      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
+      auto shared_msg = std::allocate_shared<ROSMessageType, ROSMessageTypeAllocator>(
+        ros_message_type_allocator, *message);
 
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<ROSMessageType, AllocatorT>(
           shared_msg,
           sub_ids.take_shared_subscriptions);
       }
-      if (!sub_ids.take_ownership_subscriptions.empty()) {
-        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
-          std::move(message),
-          sub_ids.take_ownership_subscriptions,
-          allocator);
-      }
+
+      this->template add_owned_msg_to_buffers<ROSMessageType, AllocatorT>(
+        std::move(message),
+        sub_ids.take_ownership_subscriptions,
+        ros_message_type_allocator);
 
       return shared_msg;
     }
   }
 
+  template<
+    typename T,
+    typename DeleterT,
+    typename MessageT,
+    typename AllocatorT,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>,
+    typename PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>,
+    typename PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type
+  >
+  typename
+  std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, PublishedType>::value, std::shared_ptr<const ROSMessageType>
+  >
+  do_intra_process_publish_and_return_shared(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<T, DeleterT> message,
+    ROSMessageTypeAllocator & ros_message_type_allocator,
+    ROSMessageTypeDeleter & ros_message_type_deleter,
+    PublishedTypeAllocator & published_type_allocator)
+  {
+    auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1);
+    ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr);
+    auto shared_ros_msg = std::shared_ptr<ROSMessageType>(ptr, ros_message_type_deleter);
+    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *shared_ros_msg);
+
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return nullptr;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // If there are no owning, just convert to shared.
+      std::shared_ptr<T> shared_msg = std::move(message);
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_blah_shared_msg_to_buffers<MessageT, T, AllocatorT>(
+          shared_msg,
+          sub_ids.take_shared_subscriptions,
+          ros_message_type_allocator,
+          ros_message_type_deleter);
+      }
+      return shared_ros_msg;
+    } else {
+      // Construct a new shared pointer from the message for the buffers that
+      // do not require ownership and to return.
+      auto shared_msg = std::allocate_shared<T, PublishedTypeAllocator>(
+        published_type_allocator,
+        *message);
+
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_blah_shared_msg_to_buffers<MessageT, T, AllocatorT>(
+          shared_msg,
+          sub_ids.take_shared_subscriptions,
+          ros_message_type_allocator,
+          ros_message_type_deleter);
+      }
+
+      this->template add_blah_owned_msg_to_buffers<MessageT, T, AllocatorT>(
+        std::move(message),
+        sub_ids.take_ownership_subscriptions,
+        ros_message_type_allocator,
+        ros_message_type_deleter);
+
+      return shared_ros_msg;
+    }
+  }
+
   /// Return true if the given rmw_gid_t matches any stored Publishers.
   RCLCPP_PUBLIC
   bool
@@ -337,9 +529,14 @@ private:
 
   template<
     typename MessageT,
-    typename Alloc,
-    typename Deleter>
-  void
+    typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type>
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<MessageT>::value &&
+    std::is_same<MessageT, ROSMessageType>::value
+  >
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
     std::vector<uint64_t> subscription_ids)
@@ -369,11 +566,69 @@ private:
     }
   }
 
+  template<
+    typename MessageT,
+    typename T,
+    typename AllocatorT,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>
+  >
+  typename
+  std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, PublishedType>::value
+  >
+  add_blah_shared_msg_to_buffers(
+    std::shared_ptr<const T> message,
+    std::vector<uint64_t> subscription_ids,
+    ROSMessageTypeAllocator & allocator,
+    ROSMessageTypeDeleter & deleter)
+  {
+    // TODO(clalancette): This goes away once the subscription buffer can take PublishedTypes
+    auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1);
+    ROSMessageTypeAllocatorTraits::construct(allocator, ptr);
+    auto shared_ros_msg = std::shared_ptr<ROSMessageType>(ptr, deleter);
+    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *shared_ros_msg);
+
+    for (auto id : subscription_ids) {
+      auto subscription_it = subscriptions_.find(id);
+      if (subscription_it == subscriptions_.end()) {
+        throw std::runtime_error("subscription has unexpectedly gone out of scope");
+      }
+      auto subscription_base = subscription_it->second.lock();
+      if (subscription_base) {
+        auto subscription = std::dynamic_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcessBuffer<ROSMessageType,
+          AllocatorT, ROSMessageTypeDeleter>
+          >(subscription_base);
+        if (nullptr == subscription) {
+          throw std::runtime_error(
+                  "failed to dynamic cast SubscriptionIntraProcessBase to "
+                  "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, which "
+                  "can happen when the publisher and subscription use different "
+                  "allocator types, which is not supported");
+        }
+
+        subscription->provide_intra_process_message(shared_ros_msg);
+      } else {
+        subscriptions_.erase(id);
+      }
+    }
+  }
+
   template<
     typename MessageT,
     typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
-  void
+    typename Deleter = std::default_delete<MessageT>,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type>
+  typename
+  std::enable_if_t<
+    rosidl_generator_traits::is_message<MessageT>::value &&
+    std::is_same<MessageT, ROSMessageType>::value
+  >
   add_owned_msg_to_buffers(
     std::unique_ptr<MessageT, Deleter> message,
     std::vector<uint64_t> subscription_ids,
@@ -419,6 +674,74 @@ private:
     }
   }
 
+  template<
+    typename MessageT,
+    typename T,
+    typename AllocatorT,
+    typename ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
+    typename PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type,
+    typename ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>,
+    typename ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type,
+    typename ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>
+  >
+  typename
+  std::enable_if_t<
+    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
+    std::is_same<T, PublishedType>::value
+  >
+  add_blah_owned_msg_to_buffers(
+    std::unique_ptr<T> message,
+    std::vector<uint64_t> subscription_ids,
+    ROSMessageTypeAllocator & allocator,
+    ROSMessageTypeDeleter & deleter)
+  {
+    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> unique_ros_msg{nullptr};
+
+    for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) {
+      auto subscription_it = subscriptions_.find(*it);
+      if (subscription_it == subscriptions_.end()) {
+        throw std::runtime_error("subscription has unexpectedly gone out of scope");
+      }
+      auto subscription_base = subscription_it->second.lock();
+      if (subscription_base) {
+        auto subscription = std::dynamic_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcessBuffer<ROSMessageType,
+          AllocatorT, ROSMessageTypeDeleter>
+          >(subscription_base);
+        if (nullptr == subscription) {
+          throw std::runtime_error(
+                  "failed to dynamic cast SubscriptionIntraProcessBase to "
+                  "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, which "
+                  "can happen when the publisher and subscription use different "
+                  "allocator types, which is not supported");
+        }
+
+        if (unique_ros_msg == nullptr) {
+          auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1);
+          ROSMessageTypeAllocatorTraits::construct(allocator, ptr);
+          unique_ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
+          rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *unique_ros_msg);
+        }
+
+        if (std::next(it) == subscription_ids.end()) {
+          // If this is the last subscription, give up ownership
+          subscription->provide_intra_process_message(std::move(unique_ros_msg));
+        } else {
+          // Copy the message since we have additional subscriptions to serve
+          using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
+          MessageUniquePtr copy_message;
+          auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1);
+          ROSMessageTypeAllocatorTraits::construct(allocator, ptr, *unique_ros_msg);
+          copy_message = MessageUniquePtr(ptr, deleter);
+
+          subscription->provide_intra_process_message(std::move(copy_message));
+        }
+      } else {
+        subscriptions_.erase(subscription_it);
+      }
+    }
+  }
+
   PublisherToSubscriptionIdsMap pub_to_subs_;
   SubscriptionMap subscriptions_;
   PublisherMap publishers_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -15,25 +15,22 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 
-#include <rmw/rmw.h>
+#include <rmw/types.h>
 
-#include <functional>
-#include <map>
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <type_traits>
 #include <utility>
 
-#include "rcl/error_handling.h"
+#include "rcl/types.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
-#include "rclcpp/experimental/create_intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -146,10 +143,10 @@ protected:
 
     if (any_callback_.use_take_shared_method()) {
       ConstMessageSharedPtr shared_msg = shared_ptr->first;
-      any_callback_.dispatch_intra_process(shared_msg, msg_info);
+      any_callback_.template dispatch_intra_process<T>(shared_msg, msg_info);
     } else {
       MessageUniquePtr unique_msg = std::move(shared_ptr->second);
-      any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
+      any_callback_.template dispatch_intra_process<T>(std::move(unique_msg), msg_info);
     }
     shared_ptr.reset();
   }

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -15,15 +15,12 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <utility>
 
-#include "rcl/error_handling.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -15,25 +15,20 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
-#include <map>
 #include <memory>
 #include <stdexcept>
 #include <string>
 #include <utility>
 
 #include "rcl/error_handling.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-#include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
-#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -131,7 +126,11 @@ protected:
   trigger_guard_condition()
   {
     rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
-    (void)ret;
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret,
+        "Failed to trigger guard condition in intra-process buffer");
+    }
   }
 
   BufferUniquePtr buffer_;

rclcpp/include/rclcpp/init_options.hpp

@@ -29,7 +29,7 @@ class InitOptions
 {
 public:
   /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
-  bool shutdown_on_sigint = true;
+  bool shutdown_on_signal = true;
 
   /// Constructor
   /**

rclcpp/include/rclcpp/loaned_message.hpp

@@ -117,7 +117,9 @@ public:
   : pub_(std::move(other.pub_)),
     message_(std::move(other.message_)),
     message_allocator_(std::move(other.message_allocator_))
-  {}
+  {
+    other.message_ = nullptr;
+  }
 
   /// Destructor of the LoanedMessage class.
   /**

rclcpp/include/rclcpp/node.hpp

@@ -148,10 +148,16 @@ public:
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
-  /// Return the list of callback groups in the node.
+  /// Iterate over the callback groups in the node, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+  void
+  for_each_callback_group(const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
 
   /// Create and return a Publisher.
   /**
@@ -979,12 +985,15 @@ public:
   std::map<std::string, std::vector<std::string>>
   get_service_names_and_types() const;
 
-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
   /**
-   * \param[in] node_name the node_name on which to count the publishers.
-   * \param[in] namespace_ the namespace of the node associated with the name
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
    */
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,11 +15,14 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
+#include <functional>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
 #include "rcl/node.h"
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -95,9 +98,16 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
 
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const override;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) override;
 
   RCLCPP_PUBLIC
   std::atomic_bool &
@@ -132,6 +142,7 @@ private:
   std::shared_ptr<rcl_node_t> node_handle_;
 
   rclcpp::CallbackGroup::SharedPtr default_callback_group_;
+  std::mutex callback_groups_mutex_;
   std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
 
   std::atomic_bool associated_with_executor_;

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -122,11 +122,19 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
 
-  /// Return list of callback groups associated with this node.
+  using CallbackGroupFunction = std::function<void (rclcpp::CallbackGroup::SharedPtr)>;
+
+  /// Iterate over the stored callback groups, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
   RCLCPP_PUBLIC
   virtual
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const = 0;
+  void
+  for_each_callback_group(const CallbackGroupFunction & func) = 0;
 
   /// Return the atomic bool which is used to ensure only one executor is used.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -70,10 +71,34 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const override;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const override;
+
   RCLCPP_PUBLIC
   std::vector<std::string>
   get_node_names() const override;
 
+  RCLCPP_PUBLIC
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const override;
+
   RCLCPP_PUBLIC
   std::vector<std::pair<std::string, std::string>>
   get_node_names_and_namespaces() const override;

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -20,6 +20,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -146,7 +147,9 @@ public:
   /**
    * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names of the topics, either announced by another nodes or by this one.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
    *
    * \param[in] no_demangle if true, topic names and types are not demangled
    */
@@ -160,7 +163,9 @@ public:
    * A service is considered to exist when at least one service server or
    * service client exists for it, whether they be local or remote to this
    * process.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names of the services, either announced by another nodes or by this one.
+   * Attempting to create clients or services using names returned by this function may not result in
+   * the desired service name being used depending on the remap rules in use.
    */
   RCLCPP_PUBLIC
   virtual
@@ -170,7 +175,9 @@ public:
   /// Return a map of existing service names to list of service types for a specific node.
   /**
    * This function only considers services - not clients.
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service clients using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
    *
    * \param[in] node_name name of the node
    * \param[in] namespace_ namespace of the node
@@ -182,18 +189,85 @@ public:
     const std::string & node_name,
     const std::string & namespace_) const = 0;
 
+  /// Return a map of existing service names and types with a specific node.
+  /**
+   * This function only considers clients - not service servers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create service servers using names returned by this function may not
+   * result in the desired service name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_client_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers publishers - not subscribers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_publisher_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
+  /// Return a map of existing topic names to list of topic types for a specific node.
+  /**
+   * This function only considers subscribers - not publishers.
+   * The returned names are the actual names after remap rules applied.
+   * Attempting to create publishers or subscribers using names returned by this function may not
+   * result in the desired topic name being used depending on the remap rules in use.
+   *
+   * \param[in] node_name name of the node
+   * \param[in] namespace_ namespace of the node
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_subscriber_names_and_types_by_node(
+    const std::string & node_name,
+    const std::string & namespace_,
+    bool no_demangle = false) const = 0;
+
   /// Return a vector of existing node names (string).
   /*
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
    */
   RCLCPP_PUBLIC
   virtual
   std::vector<std::string>
   get_node_names() const = 0;
 
+  /// Return a vector of existing node names, namespaces and enclaves (tuple of string).
+  /*
+   * The returned names are the actual names after remap rules applied.
+   * The enclaves contain the runtime security artifacts, those can be
+   * used to establish secured network.
+   * See https://design.ros2.org/articles/ros2_security_enclaves.html
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::tuple<std::string, std::string, std::string>>
+  get_node_names_with_enclaves() const = 0;
+
   /// Return a vector of existing node names and namespaces (pair of string).
   /*
-   * The returned names are the actual names used and do not have remap rules applied.
+   * The returned names are the actual names after remap rules applied.
    */
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -48,7 +48,7 @@ public:
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
     rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
-    const rclcpp::QoS & qos = rclcpp::RosoutQoS(),
+    const rclcpp::QoS & qos = rclcpp::ClockQoS(),
     bool use_clock_thread = true
   );
 

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -20,12 +20,16 @@
 #include "rcl/publisher.h"
 #include "rcl/subscription.h"
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
-#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_base.hpp"
 #include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_factory.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp

rclcpp/include/rclcpp/node_options.hpp

@@ -349,6 +349,9 @@ public:
    * global arguments (e.g. parameter overrides from a YAML file), which are
    * not explicitly declared will not appear on the node at all, even if
    * `allow_undeclared_parameters` is true.
+   * Parameter declaration from overrides is done in the node's base constructor,
+   * so the user must take care to check if the parameter is already (e.g.
+   * automatically) declared before declaring it themselves.
    * Already declared parameters will not be re-declared, and parameters
    * declared in this way will use the default constructed ParameterDescriptor.
    */

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -169,13 +169,17 @@ public:
     NodeT node,
     const rclcpp::QoS & qos =
     rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
+  : node_base_(rclcpp::node_interfaces::get_node_base_interface(node))
   {
-    node_base_ = rclcpp::node_interfaces::get_node_base_interface(node);
     auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);
 
+    callbacks_ = std::make_shared<Callbacks>();
+
     event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
       node_topics, "/parameter_events", qos,
-      std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1));
+      [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
+        callbacks->event_callback(event);
+      });
   }
 
   using ParameterEventCallbackType =
@@ -249,8 +253,8 @@ public:
   get_parameter_from_event(
     const rcl_interfaces::msg::ParameterEvent & event,
     rclcpp::Parameter & parameter,
-    const std::string parameter_name,
-    const std::string node_name = "");
+    const std::string & parameter_name,
+    const std::string & node_name = "");
 
   /// Get an rclcpp::Parameter from parameter event
   /**
@@ -269,8 +273,8 @@ public:
   static rclcpp::Parameter
   get_parameter_from_event(
     const rcl_interfaces::msg::ParameterEvent & event,
-    const std::string parameter_name,
-    const std::string node_name = "");
+    const std::string & parameter_name,
+    const std::string & node_name = "");
 
   /// Get all rclcpp::Parameter values from a parameter event
   /**
@@ -285,17 +289,6 @@ public:
   using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
 
 protected:
-  /// Callback for parameter events subscriptions.
-  RCLCPP_PUBLIC
-  void
-  event_callback(const rcl_interfaces::msg::ParameterEvent & event);
-
-  // Utility function for resolving node path.
-  std::string resolve_path(const std::string & path);
-
-  // Node interface used for base functionality
-  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
-
   // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
   // Hash function for string pair required in std::unordered_map
   // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
@@ -319,18 +312,34 @@ protected:
   };
   // *INDENT-ON*
 
-  // Map container for registered parameters
-  std::unordered_map<
-    std::pair<std::string, std::string>,
-    CallbacksContainerType,
-    StringPairHash
-  > parameter_callbacks_;
+  struct Callbacks
+  {
+    std::recursive_mutex mutex_;
+
+    // Map container for registered parameters
+    std::unordered_map<
+      std::pair<std::string, std::string>,
+      CallbacksContainerType,
+      StringPairHash
+    > parameter_callbacks_;
+
+    std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
+
+    /// Callback for parameter events subscriptions.
+    RCLCPP_PUBLIC
+    void
+    event_callback(const rcl_interfaces::msg::ParameterEvent & event);
+  };
+
+  std::shared_ptr<Callbacks> callbacks_;
+
+  // Utility function for resolving node path.
+  std::string resolve_path(const std::string & path);
+
+  // Node interface used for base functionality
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
 
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
-
-  std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
-
-  std::recursive_mutex mutex_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/parameter_value.hpp

@@ -301,6 +301,16 @@ public:
     return get<ParameterType::PARAMETER_BOOL_ARRAY>();
   }
 
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<int> &>::value, const std::vector<int64_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
+  }
+
   template<typename type>
   constexpr
   typename std::enable_if<
@@ -311,6 +321,16 @@ public:
     return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
   }
 
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<float> &>::value, const std::vector<double> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  }
+
   template<typename type>
   constexpr
   typename std::enable_if<

rclcpp/include/rclcpp/publisher.hpp

@@ -20,12 +20,14 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <type_traits>
 #include <utility>
 
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rosidl_runtime_cpp/traits.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
@@ -253,7 +255,7 @@ public:
       this->do_inter_process_publish(*msg);
       return;
     }
-    // If an interprocess subscription exist, then the unique_ptr is promoted
+    // If an interprocess subscription exists, then the unique_ptr is promoted
     // to a shared_ptr and published.
     // This allows doing the intraprocess publish first and then doing the
     // interprocess publish, resulting in lower publish-to-subscribe latency.
@@ -263,10 +265,11 @@ public:
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));
+      auto shared_msg = this->do_intra_process_publish_and_return_shared<T, ROSMessageTypeDeleter>(
+        std::move(msg));
       this->do_inter_process_publish(*shared_msg);
     } else {
-      this->do_intra_process_publish(std::move(msg));
+      this->do_intra_process_publish<T, ROSMessageTypeDeleter>(std::move(msg));
     }
   }
 
@@ -319,13 +322,30 @@ public:
   >
   publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    if (!intra_process_is_enabled_) {
+      // If we aren't using intraprocess at all, do the type conversion
+      // immediately and publish interprocess.
+      auto unique_ros_msg = this->create_ros_message_unique_ptr();
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *unique_ros_msg);
+      this->do_inter_process_publish(*unique_ros_msg);
+      return;
+    }
+    // If an interprocess subscription exists, then the unique_ptr is promoted
+    // to a shared_ptr and published.
+    // This allows doing the intraprocess publish first and then doing the
+    // interprocess publish, resulting in lower publish-to-subscribe latency.
+    // It's not possible to do that with an unique_ptr,
+    // as do_intra_process_publish takes the ownership of the message.
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+
+    if (inter_process_publish_needed) {
+      auto shared_msg = this->do_intra_process_publish_and_return_shared<T, PublishedTypeDeleter>(
+        std::move(msg));
+      this->do_inter_process_publish(*shared_msg);
+    } else {
+      this->do_intra_process_publish<T, PublishedTypeDeleter>(std::move(msg));
+    }
   }
 
   /// Publish a message on the topic.
@@ -346,12 +366,7 @@ public:
   >
   publish(const T & msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-
-    // Avoid allocating when not using intra process.
+    // Avoid double allocation when not using intra process.
     if (!intra_process_is_enabled_) {
       // Convert to the ROS message equivalent and publish it.
       ROSMessageType ros_msg;
@@ -359,13 +374,11 @@ public:
       // In this case we're not using intra process.
       return this->do_inter_process_publish(ros_msg);
     }
-    // Otherwise we have to allocate memory in a unique_ptr, convert it,
-    // and pass it along.
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     // As the message is not const, a copy should be made.
     // A shared_ptr<const MessageT> could also be constructed here.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg);
+    this->publish(std::move(unique_msg));
   }
 
   void
@@ -438,10 +451,7 @@ protected:
   void
   do_inter_process_publish(const ROSMessageType & msg)
   {
-    TRACEPOINT(
-      rclcpp_publish,
-      static_cast<const void *>(publisher_handle_.get()),
-      static_cast<const void *>(&msg));
+    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
     auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -493,8 +503,9 @@ protected:
     }
   }
 
+  template<typename T, typename DeleterT>
   void
-  do_intra_process_publish(std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  do_intra_process_publish(std::unique_ptr<T, DeleterT> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -505,15 +516,17 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    ipm->template do_intra_process_publish<ROSMessageType, AllocatorT>(
+    ipm->template do_intra_process_publish<T, DeleterT, MessageT, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_);
+      ros_message_type_allocator_,
+      ros_message_type_deleter_,
+      published_type_allocator_);
   }
 
+  template<typename T, typename DeleterT>
   std::shared_ptr<const ROSMessageType>
-  do_intra_process_publish_and_return_shared(
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  do_intra_process_publish_and_return_shared(std::unique_ptr<T, DeleterT> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -524,11 +537,13 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType,
+    return ipm->template do_intra_process_publish_and_return_shared<T, DeleterT, MessageT,
              AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_);
+      ros_message_type_allocator_,
+      ros_message_type_deleter_,
+      published_type_allocator_);
   }
 
   /// Return a new unique_ptr using the ROSMessageType of the publisher.
@@ -540,7 +555,7 @@ protected:
     return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
   }
 
-  /// Duplicate a given ros message as a unique_ptr.
+  /// Duplicate a given ROS message as a unique_ptr.
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
   duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg)
   {
@@ -549,6 +564,15 @@ protected:
     return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
   }
 
+  /// Duplicate a given type adapted message as a unique_ptr.
+  std::unique_ptr<PublishedType, PublishedTypeDeleter>
+  duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg)
+  {
+    auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1);
+    PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg);
+    return std::unique_ptr<PublishedType, PublishedTypeDeleter>(ptr, published_type_deleter_);
+  }
+
   /// Copy of original options passed during construction.
   /**
    * It is important to save a copy of this so that the rmw payload which it

rclcpp/include/rclcpp/rclcpp.hpp

@@ -140,7 +140,6 @@
  *   - rclcpp/duration.hpp
  *   - rclcpp/function_traits.hpp
  *   - rclcpp/macros.hpp
- *   - rclcpp/scope_exit.hpp
  *   - rclcpp/time.hpp
  *   - rclcpp/utilities.hpp
  *   - rclcpp/typesupport_helpers.hpp

rclcpp/include/rclcpp/scope_exit.hpp

@@ -19,6 +19,10 @@
 #ifndef RCLCPP__SCOPE_EXIT_HPP_
 #define RCLCPP__SCOPE_EXIT_HPP_
 
+// TODO(christophebedard) remove this header completely in I-turtle
+
+#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead
+
 #include <functional>
 
 #include "rclcpp/macros.hpp"

rclcpp/include/rclcpp/service.hpp

@@ -141,7 +141,9 @@ protected:
 };
 
 template<typename ServiceT>
-class Service : public ServiceBase
+class Service
+  : public ServiceBase,
+  public std::enable_shared_from_this<Service<ServiceT>>
 {
 public:
   using CallbackType = std::function<
@@ -335,9 +337,10 @@ public:
     std::shared_ptr<void> request) override
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
-    auto response = std::make_shared<typename ServiceT::Response>();
-    any_callback_.dispatch(request_header, typed_request, response);
-    send_response(*request_header, *response);
+    auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request);
+    if (response) {
+      send_response(*request_header, *response);
+    }
   }
 
   void

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -388,6 +388,11 @@ public:
           ++it;
           continue;
         }
+        if (!timer->call()) {
+          // timer was cancelled, skip it.
+          ++it;
+          continue;
+        }
         // Otherwise it is safe to set and return the any_exec
         any_exec.timer = timer;
         any_exec.callback_group = group;
@@ -395,7 +400,7 @@ public:
         timer_handles_.erase(it);
         return;
       }
-      // Else, the service is no longer valid, remove it and continue
+      // Else, the timer is no longer valid, remove it and continue
       it = timer_handles_.erase(it);
     }
   }

rclcpp/include/rclcpp/subscription_base.hpp

@@ -107,7 +107,7 @@ public:
   /// Get the actual QoS settings, after the defaults have been determined.
   /**
    * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
-   * can only be resolved after the creation of the publisher, and it
+   * can only be resolved after the creation of the subscription, and it
    * depends on the underlying rmw implementation.
    * If the underlying setting in use can't be represented in ROS terms,
    * it will be set to RMW_QOS_POLICY_*_UNKNOWN.

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -26,14 +26,13 @@
 
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/get_message_type_support_handle.hpp"
-#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
-#include "rclcpp/visibility_control.hpp"
 #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {

rclcpp/include/rclcpp/time_source.hpp

@@ -74,6 +74,14 @@ public:
     const rclcpp::QoS & qos = rclcpp::ClockQoS(),
     bool use_clock_thread = true);
 
+  // The TimeSource is uncopyable
+  TimeSource(const TimeSource &) = delete;
+  TimeSource & operator=(const TimeSource &) = delete;
+
+  // The TimeSource is moveable
+  TimeSource(TimeSource &&) = default;
+  TimeSource & operator=(TimeSource &&) = default;
+
   /// Attach node to the time source.
   /**
    * \param node std::shared pointer to a initialized node
@@ -116,89 +124,36 @@ public:
   RCLCPP_PUBLIC
   void attachClock(rclcpp::Clock::SharedPtr clock);
 
-  /// Detach a clock to the time source
+  /// Detach a clock from the time source
   RCLCPP_PUBLIC
   void detachClock(rclcpp::Clock::SharedPtr clock);
 
+  /// Get whether a separate clock thread is used or not
+  RCLCPP_PUBLIC
+  bool get_use_clock_thread();
+
+  /// Set whether to use a separate clock thread or not
+  RCLCPP_PUBLIC
+  void set_use_clock_thread(bool use_clock_thread);
+
+  /// Check if the clock thread is joinable
+  RCLCPP_PUBLIC
+  bool clock_thread_is_joinable();
+
   /// TimeSource Destructor
   RCLCPP_PUBLIC
   ~TimeSource();
 
-protected:
-  // Dedicated thread for clock subscription.
-  bool use_clock_thread_;
-  std::thread clock_executor_thread_;
-
 private:
-  // Preserve the node reference
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
-  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::NodeClockInterface::SharedPtr node_clock_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  class ClocksState;
+  std::shared_ptr<ClocksState> clocks_state_;
 
-  // Store (and update on node attach) logger for logging.
-  Logger logger_;
+  class NodeState;
+  std::shared_ptr<NodeState> node_state_;
 
-  // QoS of the clock subscription.
-  rclcpp::QoS qos_;
-
-  // The subscription for the clock callback
-  using MessageT = rosgraph_msgs::msg::Clock;
-  using Alloc = std::allocator<void>;
-  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
-  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
-  std::mutex clock_sub_lock_;
-  rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
-  rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_;
-  std::promise<void> cancel_clock_executor_promise_;
-
-  // The clock callback itself
-  void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg);
-
-  // Create the subscription for the clock topic
-  void create_clock_sub();
-
-  // Destroy the subscription for the clock topic
-  void destroy_clock_sub();
-
-  // Parameter Event subscription
-  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
-  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
-  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
-
-  // Callback for parameter updates
-  void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event);
-
-  // An enum to hold the parameter state
-  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
-  UseSimTimeParameterState parameter_state_;
-
-  // An internal method to use in the clock callback that iterates and enables all clocks
-  void enable_ros_time();
-  // An internal method to use in the clock callback that iterates and disables all clocks
-  void disable_ros_time();
-
-  // Internal helper functions used inside iterators
-  static void set_clock(
-    const builtin_interfaces::msg::Time::SharedPtr msg,
-    bool set_ros_time_enabled,
-    rclcpp::Clock::SharedPtr clock);
-
-  // Local storage of validity of ROS time
-  // 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_;
-
-  // A lock to protect iterating the associated_clocks_ field.
-  std::mutex clock_list_lock_;
-  // A vector to store references to associated clocks.
-  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
-  // A handler for the use_sim_time parameter callback.
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
+  // Preserve the arguments received by the constructor for reuse at runtime
+  bool constructed_use_clock_thread_;
+  rclcpp::QoS constructed_qos_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/timer.hpp

@@ -91,6 +91,17 @@ public:
   void
   reset();
 
+  /// Indicate that we're about to execute the callback.
+  /**
+   * The multithreaded executor takes advantage of this to avoid scheduling
+   * the callback multiple times.
+   *
+   * \return `true` if the callback should be executed, `false` if the timer was canceled.
+   */
+  RCLCPP_PUBLIC
+  virtual bool
+  call() = 0;
+
   /// Call the callback function when the timer signal is emitted.
   RCLCPP_PUBLIC
   virtual void
@@ -192,19 +203,28 @@ public:
   }
 
   /**
-   * \sa rclcpp::TimerBase::execute_callback
-   * \throws std::runtime_error if it failed to notify timer that callback occurred
+   * \sa rclcpp::TimerBase::call
+   * \throws std::runtime_error if it failed to notify timer that callback will occurr
    */
-  void
-  execute_callback() override
+  bool
+  call() override
   {
     rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
-      return;
+      return false;
     }
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
+    return true;
+  }
+
+  /**
+   * \sa rclcpp::TimerBase::execute_callback
+   */
+  void
+  execute_callback() override
+  {
     TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
     TRACEPOINT(callback_end, static_cast<const void *>(&callback_));

rclcpp/include/rclcpp/utilities.hpp

@@ -42,6 +42,20 @@ std::string to_string(T value)
 
 namespace rclcpp
 {
+
+/// Option to indicate which signal handlers rclcpp should install.
+enum class SignalHandlerOptions
+{
+  /// Install both sigint and sigterm, this is the default behavior.
+  All,
+  /// Install only a sigint handler.
+  SigInt,
+  /// Install only a sigterm handler.
+  SigTerm,
+  /// Do not install any signal handler.
+  None,
+};
+
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * Initializes the global context which is accessible via the function
@@ -50,10 +64,16 @@ namespace rclcpp
  * rclcpp::install_signal_handlers().
  *
  * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
+ *
+ * \param signal_handler_options option to indicate which signal handlers should be installed.
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions());
+init(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options = InitOptions(),
+  SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
 /// Install the global signal handler for rclcpp.
 /**
@@ -67,17 +87,26 @@ init(int argc, char const * const argv[], const InitOptions & init_options = Ini
  *
  * This function is thread-safe.
  *
+ * \param signal_handler_options option to indicate which signal handlers should be installed.
  * \return true if signal handler was installed by this function, false if already installed.
  */
 RCLCPP_PUBLIC
 bool
-install_signal_handlers();
+install_signal_handlers(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
 /// Return true if the signal handlers are installed, otherwise false.
 RCLCPP_PUBLIC
 bool
 signal_handlers_installed();
 
+/// Get the current signal handler options.
+/**
+ * If no signal handler is installed, SignalHandlerOptions::None is returned.
+ */
+RCLCPP_PUBLIC
+SignalHandlerOptions
+get_current_signal_handler_options();
+
 /// Uninstall the global signal handler for rclcpp.
 /**
  * This function does not necessarily need to be called, but can be used to

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -0,0 +1,100 @@
+// Copyright 2021 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__WAIT_FOR_MESSAGE_HPP_
+#define RCLCPP__WAIT_FOR_MESSAGE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
+
+namespace rclcpp
+{
+/// Wait for the next incoming message.
+/**
+ * Given an already initialized subscription,
+ * wait for the next incoming message to arrive before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] subscription shared pointer to a previously initialized subscription.
+ * \param[in] context shared pointer to a context to watch for SIGINT requests.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription,
+  std::shared_ptr<rclcpp::Context> context,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+  auto shutdown_callback_handle = context->add_on_shutdown_callback(
+    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{gc}]() {
+      auto strong_gc = weak_gc.lock();
+      if (strong_gc) {
+        strong_gc->trigger();
+      }
+    });
+
+  rclcpp::WaitSet wait_set;
+  wait_set.add_subscription(subscription);
+  wait_set.add_guard_condition(gc);
+  auto ret = wait_set.wait(time_to_wait);
+  if (ret.kind() != rclcpp::WaitResultKind::Ready) {
+    return false;
+  }
+
+  if (wait_set.get_rcl_wait_set().guard_conditions[0]) {
+    return false;
+  }
+
+  rclcpp::MessageInfo info;
+  if (!subscription->take(out, info)) {
+    return false;
+  }
+
+  return true;
+}
+
+/// Wait for the next incoming message.
+/**
+ * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] node the node pointer to initialize the subscription on.
+ * \param[in] topic the topic to wait for messages.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  rclcpp::Node::SharedPtr node,
+  const std::string & topic,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<const MsgT>) {});
+  return wait_for_message<MsgT, Rep, Period>(
+    out, sub, node->get_node_options().context(), time_to_wait);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_FOR_MESSAGE_HPP_

rclcpp/include/rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp

@@ -1,43 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
-
-#include <stdexcept>
-
-#include "rmw/impl/cpp/demangle.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-
-class AlreadyAssociatedWithWaitSetException : public std::runtime_error
-{
-public:
-  template<typename EntityT>
-  explicit
-  AlreadyAssociatedWithWaitSetException(const EntityT & entity_instance)
-  : std::runtime_error(
-    "cannot associate " +
-    rmw::impl::cpp::demangle(entity_instance) +
-    " with wait set because it is already in use by a wait set")
-  {}
-};
-
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/client_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/client.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using ClientEntry =
-  // EntityEntryTemplate<rclcpp::ClientBase, std::shared_ptr<rclcpp::ClientBase>>;
-  EntityEntryTemplate<rclcpp::ClientBase>;
-using WeakClientEntry =
-  // EntityEntryTemplate<rclcpp::ClientBase, std::weak_ptr<rclcpp::ClientBase>>;
-  EntityEntryTemplate<rclcpp::ClientBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/entity_entry.hpp

@@ -1,183 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
-
-#include <cassert>
-#include <memory>
-
-#include "rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-// Forward declaration for use in friend statement.
-template<typename EntityT>
-class ManagedEntityEntryTemplate;
-
-/// Encapsulating class for wait set entities, and gateway to the ManagedEntityEntryTemplate.
-/**
- * The entity is stored as a std::shared_ptr.
- *
- * This is class can be converted to a "managed" version which ensures the
- * entity is not associated with another wait set already, then associates it
- * with the current wait set, and then dissociates it on destruction.
- */
-template<typename EntityT>
-class EntityEntryTemplate
-{
-public:
-  EntityEntryTemplate(std::shared_ptr<EntityT> entity_in = nullptr)
-  : entity_(entity_in)
-  {}
-private:
-  std::shared_ptr<EntityT> entity_;
-
-  friend ManagedEntityEntryTemplate<EntityT>;
-};
-
-/// Managing class for wait set entities, with RAII-style (dis)association with the wait set.
-/**
- * The entity is stored as a std::shared_ptr, but ths class can be converted
- * (one way) into a weak version that stores it as a std::weak_ptr.
- *
- * This class will assert that the entity is not already associated with a
- * wait set, while atomically indicating it is associated with this wait set
- * to prevent other wait sets from using it, and then on destruction this class
- * will disassociate it.
- *
- * \throws rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException if entity
- *   is already associated with a wait set.
- */
-template<typename EntityT>
-class ManagedEntityEntryTemplate
-{
-public:
-  /// The only valid way to construct this is with an unmanaged entity entry.
-  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
-  : entity_(unmanaged_entity_entry.entity_)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-protected:
-  std::shared_ptr<EntityT> entity_;
-
-};
-
-/// Version of ManagedEntityEntryTemplate with weak ownership and best effort disassociation.
-/**
- * The entity is stored as a std::weak_ptr, but on destruction, the entity is
- * locked, and if not nullptr, then it will be marked as not in use by a wait set.
- */
-template<typename EntityT>
-class WeakManagedEntityEntryTemplate
-{
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-
-};
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/guard_condition_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/guard_condition.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using GuardConditionEntry =
-  // EntityEntryTemplate<rclcpp::GuardCondition, std::shared_ptr<rclcpp::GuardCondition>>;
-  EntityEntryTemplate<rclcpp::GuardCondition>;
-using WeakGuardConditionEntry =
-  // EntityEntryTemplate<rclcpp::GuardCondition, std::weak_ptr<rclcpp::GuardCondition>>;
-  EntityEntryTemplate<rclcpp::GuardCondition>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/service_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/service.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using ServiceEntry =
-  // EntityEntryTemplate<rclcpp::ServiceBase, std::shared_ptr<rclcpp::ServiceBase>>;
-  EntityEntryTemplate<rclcpp::ServiceBase>;
-using WeakServiceEntry =
-  // EntityEntryTemplate<rclcpp::ServiceBase, std::weak_ptr<rclcpp::ServiceBase>>;
-  EntityEntryTemplate<rclcpp::ServiceBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/subscription_entry.hpp

@@ -1,403 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
-
-#include <memory>
-#include <optional>
-#include <vector>
-
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/subscription_wait_set_mask.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
-template<>
-class EntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-public:
-  EntityEntryTemplate(
-    std::shared_ptr<EntityT> entity_in = nullptr,
-    const rclcpp::SubscriptionWaitSetMask & mask_in = {})
-  : entity_(entity_in),
-    mask_(mask_in)
-  {}
-
-  /// Create ManagedEntityEntryTemplate instances for the subscription and waitables if needed.
-  /**
-   * This method uses the SubscriptionWaitSetMask to determine how to decompose
-   * the various entities within the subscription into managed entries.
-   *
-   * It optionally returns a managed entry for the subscription, if the mask
-   * indicates it should, and then optionally any waitables for the
-   * intra-process communication and QoS events.
-   */
-  std::pair<
-    std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
-    std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
-  >
-  manage();
-  // defined below ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>.
-
-private:
-  std::shared_ptr<EntityT> entity_;
-  rclcpp::SubscriptionWaitSetMask mask_;
-};
-
-/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
-template<>
-class ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-  /// Only constructed by EntityEntryTemplate<rclcpp::SubscriptionBase>::manage().
-  explicit ManagedEntityEntryTemplate(std::shared_ptr<EntityT> subscription)
-  : entity_(subscription)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-public:
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-private:
-  std::shared_ptr<EntityT> entity_;
-
-  friend EntityEntryTemplate<EntityT>;
-
-};
-
-std::pair<
-  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
-  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
->
-EntityEntryTemplate<rclcpp::SubscriptionBase>::manage()
-{
-  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>> managed_subscription_entry;
-  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>> waitables;
-
-  if (mask_.include_subscription) {
-    managed_subscription_entry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>(entity_);
-  }
-  if (mask_.include_events) {
-    for (const auto & event_waitable : entity_->get_event_handlers()) {
-      waitables.emplace_back(EntityEntryTemplate<rclcpp::Waitable>(event_waitable, entity_));
-    }
-  }
-  if (mask_.include_intra_process_waitable) {
-    waitables.emplace_back(
-      EntityEntryTemplate<rclcpp::Waitable>(entity_->get_intra_process_waitable(), entity_)
-    );
-  }
-
-  return {managed_subscription_entry, waitables};
-}
-
-/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
-template<>
-class WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
-{
-  using EntityT = rclcpp::SubscriptionBase;
-
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(entity.get(), false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-  std::weak_ptr<void> weak_associated_entity_;
-
-};
-
-using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
-using ManagedSubscriptionEntry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
-using WeakManagedSubscriptionEntry = WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
-
-// /// Specialization for Subscriptions.
-// template<>
-// class EntityEntryTemplate<rclcpp::SubscriptionBase>
-// {
-//   using EntityT = rclcpp::SubscriptionBase;
-//   std::shared_ptr<EntityT> entity_;
-//   rclcpp::SubscriptionWaitSetMask mask_;
-
-// public:
-//   EntityEntryTemplate(
-//     std::shared_ptr<EntityT> entity_in = nullptr,
-//     const rclcpp::SubscriptionWaitSetMask & mask_in = {})
-//   : entity_(entity_in),
-//     mask_(mask_in)
-//   {}
-
-//   ~EntityEntryTemplate()
-//   {
-//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity_, mask_);
-//     }
-//   }
-
-//   /// See EntityEntryTemplate::get_entity().
-//   std::shared_ptr<EntityT>
-//   get_entity() const
-//   {
-//     return entity_;
-//   }
-
-//   /// Return a const reference to the subscrption mask.
-//   const rclcpp::SubscriptionWaitSetMask &
-//   get_mask() const
-//   {
-//     return mask_;
-//   }
-
-//   /// See EntityEntryTemplate::manage().
-//   std::vector<WaitableEntry>
-//   manage()
-//   {
-//     if (nullptr == entity_) {
-//       throw std::runtime_error("manage() called on EntityEntry with null entity");
-//     }
-//     auto associate = [this](void * entity_part) {
-//       bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_part, true);
-//       if (already_in_use) {
-//         throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-//       }
-//     };
-
-//     if (mask_.include_subscription) {
-//       associate(entity_.get());
-//     }
-
-//     std::vector<WaitableEntry> decomposed_waitables;
-
-//     if (mask_.include_events) {
-//       for (auto event : entity_->get_event_handlers()) {
-//         // TODO(wjwwood): do we need to use the unmanage_function argument
-//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
-//         decomposed_waitables.emplace_back(event, entity_);
-//         decomposed_waitables.back().manage();
-//       }
-//       // mask_.include_events = false;
-//     }
-//     if (mask_.include_intra_process_waitable) {
-//       auto waitable = entity_->get_intra_process_waitable();
-//       if (nullptr != waitable) {
-//         // TODO(wjwwood): do we need to use the unmanage_function argument
-//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
-//         decomposed_waitables.emplace_back(waitable, entity_);
-//         decomposed_waitables.back().manage();
-//         // mask_.include_intra_process_waitable = false;
-//       }
-//     }
-
-//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = true;
-
-//     return decomposed_waitables;
-//   }
-
-//   /// See EntityEntryTemplate::reset().
-//   void
-//   reset() noexcept
-//   {
-//     entity_.reset();
-//   }
-
-// protected:
-//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
-
-//   static
-//   void
-//   cleanup(std::shared_ptr<EntityT> subscription, rclcpp::SubscriptionWaitSetMask mask)
-//   {
-//     if (subscription != nullptr) {
-//       auto dissociate = [&subscription](void * entity_part) {
-//         bool was_in_use = subscription->exchange_in_use_by_wait_set_state(entity_part, false);
-//         assert(was_in_use);
-//       };
-//       if (mask.include_subscription) {
-//         dissociate(subscription.get());
-//       }
-//       if (mask.include_events) {
-//         for (auto event : subscription->get_event_handlers()) {
-//           dissociate(event.get());
-//         }
-//       }
-//       if (mask.include_intra_process_waitable) {
-//         auto waitable = subscription->get_intra_process_waitable();
-//         if (nullptr != waitable) {
-//           dissociate(waitable.get());
-//         }
-//       }
-//     }
-//   }
-
-//   friend WeakEntityEntryTemplate<EntityT>;
-
-// };
-
-// /// Specialization for Subscriptions.
-// template<>
-// class WeakEntityEntryTemplate<rclcpp::SubscriptionBase>
-// {
-//   using EntityT = rclcpp::SubscriptionBase;
-//   std::weak_ptr<EntityT> weak_entity_;
-//   rclcpp::SubscriptionWaitSetMask mask_;
-
-// public:
-//   explicit WeakEntityEntryTemplate(EntityEntryTemplate<EntityT> && moved_entity_entry)
-//   : weak_entity_(moved_entity_entry.get_entity()),
-//     mask_(moved_entity_entry.mask_),
-//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_(
-//       moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_
-//     )
-//   {
-//     moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = false;
-//   }
-
-//   ~WeakEntityEntryTemplate()
-//   {
-//     auto entity = weak_entity_.lock();
-//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity, mask_);
-//     }
-//   }
-
-//   /// See WeakEntityEntryTemplate::get_weak_entity().
-//   std::weak_ptr<EntityT>
-//   get_weak_entity() const
-//   {
-//     return weak_entity_;
-//   }
-
-//   /// Return a const reference to the subscrption mask.
-//   const rclcpp::SubscriptionWaitSetMask &
-//   get_mask() const
-//   {
-//     return mask_;
-//   }
-
-//   /// See WeakEntityEntryTemplate::lock().
-//   std::shared_ptr<EntityT>
-//   lock() const
-//   {
-//     return weak_entity_.lock();
-//   }
-
-//   /// See WeakEntityEntryTemplate::expired().
-//   bool
-//   expired() const noexcept
-//   {
-//     return weak_entity_.expired();
-//   }
-
-// protected:
-//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
-
-// };
-
-// using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
-// using WeakSubscriptionEntry = WeakEntityEntryTemplate<rclcpp::SubscriptionBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/timer_entry.hpp

@@ -1,41 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_
-
-#include <memory>
-
-#include "rclcpp/timer.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-using TimerEntry =
-  // EntityEntryTemplate<rclcpp::TimerBase, std::shared_ptr<rclcpp::TimerBase>>;
-  EntityEntryTemplate<rclcpp::TimerBase>;
-using WeakTimerEntry =
-  // EntityEntryTemplate<rclcpp::TimerBase, std::weak_ptr<rclcpp::TimerBase>>;
-  EntityEntryTemplate<rclcpp::TimerBase>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/waitable_entry.hpp

@@ -1,183 +0,0 @@
-// Copyright 2021 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__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
-#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
-
-#include <functional>
-#include <memory>
-#include <type_traits>
-
-#include "rclcpp/waitable.hpp"
-#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
-
-namespace rclcpp
-{
-namespace wait_set_policies
-{
-namespace detail
-{
-
-/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
-template<>
-class EntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  EntityEntryTemplate(
-    std::shared_ptr<EntityT> entity_in = nullptr,
-    std::shared_ptr<void> associated_entity_in = nullptr)
-  : entity_(entity_in),
-    associated_entity_(associated_entity_in)
-  {}
-private:
-  std::shared_ptr<EntityT> entity_;
-  std::shared_ptr<void> associated_entity_;
-
-  friend ManagedEntityEntryTemplate<EntityT>;
-};
-
-/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
-template<>
-class ManagedEntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  /// The only valid way to construct this is with an unmanaged entity entry.
-  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
-  : entity_(unmanaged_entity_entry.entity_),
-    associated_entity_(unmanaged_entity_entry.associated_entity_)
-  {
-    if (nullptr == entity_) {
-      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
-    }
-    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
-    if (already_in_use) {
-      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
-    }
-  }
-
-  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
-  // {
-  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
-  //     throw std::runtime_error("")
-  //   }
-  // }
-
-  ~ManagedEntityEntryTemplate()
-  {
-    if ((nullptr != entity_)) {
-      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity shared pointer.
-  std::shared_ptr<EntityT>
-  get_entity() const
-  {
-    return entity_;
-  }
-
-  /// Return the interal associated entity shared pointer.
-  std::shared_ptr<void>
-  get_associated_entity() const
-  {
-    return associated_entity_;
-  }
-
-  /// Reset the entity.
-  /**
-   * Specializations of this class may reset more than one item.
-   * Having this method in all instantiations of this class provides uniform access.
-   */
-  // void
-  // reset() noexcept
-  // {
-  //   entity_.reset();
-  // }
-
-protected:
-  std::shared_ptr<EntityT> entity_;
-  std::shared_ptr<void> associated_entity_;
-
-};
-
-/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
-template<>
-class WeakManagedEntityEntryTemplate<rclcpp::Waitable>
-{
-  using EntityT = rclcpp::Waitable;
-
-public:
-  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
-  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
-  : weak_entity_(moved_entity_entry.get_entity()),
-    weak_associated_entity_(moved_entity_entry.get_associated_entity())
-  {}
-
-  ~WeakManagedEntityEntryTemplate()
-  {
-    auto entity = weak_entity_.lock();
-    if (nullptr != entity) {
-      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
-      assert(was_in_use);
-    }
-  }
-
-  /// Return the interal entity weak pointer.
-  std::weak_ptr<EntityT>
-  get_weak_entity() const
-  {
-    return weak_entity_;
-  }
-
-  // /// Lock the entity.
-  // /**
-  //  * Specializations of this class may select from more than one item to lock.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // std::shared_ptr<EntityT>
-  // lock() const
-  // {
-  //   return weak_entity_.lock();
-  // }
-
-  // /// Return true if the entity has expired, otherwise false.
-  // /**
-  //  * Specializations of this class may select from more than one item to check.
-  //  * Having this method in all instantiations of this class provides uniform access.
-  //  */
-  // bool
-  // expired() const noexcept
-  // {
-  //   return weak_entity_.expired();
-  // }
-private:
-  std::weak_ptr<EntityT> weak_entity_;
-  std::weak_ptr<void> weak_associated_entity_;
-
-};
-
-using WaitableEntry = EntityEntryTemplate<rclcpp::Waitable>;
-using ManagedWaitableEntry = ManagedEntityEntryTemplate<rclcpp::Waitable>;
-using WeakManagedWaitableEntry = WeakManagedEntityEntryTemplate<rclcpp::Waitable>;
-
-}  // namespace detail
-}  // namespace wait_set_policies
-}  // namespace rclcpp
-
-#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -28,13 +28,7 @@
 #include "rclcpp/subscription_wait_set_mask.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set_policies/detail/client_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/guard_condition_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/service_entry.hpp"
 #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
-#include "rclcpp/wait_set_policies/detail/subscription_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/timer_entry.hpp"
-#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -48,27 +42,124 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo
 protected:
   using is_mutable = std::true_type;
 
-  using WeakManagedSubscriptionEntry = detail::WeakManagedSubscriptionEntry;
-  using SubscriptionEntry = detail::SubscriptionEntry;
+  class SubscriptionEntry
+  {
+    // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
 
-  using SequenceOfWeakSubscriptions = std::vector<WeakManagedSubscriptionEntry>;
+public:
+    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    /// Conversion constructor, which is intentionally not marked explicit.
+    SubscriptionEntry(
+      std::shared_ptr<rclcpp::SubscriptionBase> subscription_in = nullptr,
+      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+    : subscription(std::move(subscription_in)),
+      mask(mask_in)
+    {}
+
+    void
+    reset() noexcept
+    {
+      subscription.reset();
+    }
+  };
+  class WeakSubscriptionEntry
+  {
+public:
+    std::weak_ptr<rclcpp::SubscriptionBase> subscription;
+    rclcpp::SubscriptionWaitSetMask mask;
+
+    explicit WeakSubscriptionEntry(
+      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in,
+      const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept
+    : subscription(subscription_in),
+      mask(mask_in)
+    {}
+
+    explicit WeakSubscriptionEntry(const SubscriptionEntry & other)
+    : subscription(other.subscription),
+      mask(other.mask)
+    {}
+
+    std::shared_ptr<rclcpp::SubscriptionBase>
+    lock() const
+    {
+      return subscription.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return subscription.expired();
+    }
+  };
+  using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
   using SubscriptionsIterable = std::vector<SubscriptionEntry>;
 
-  using SequenceOfWeakGuardConditions = std::vector<detail::WeakGuardConditionEntry>;
-  using GuardConditionsIterable = std::vector<detail::GuardConditionEntry>;
+  using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
+  using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
 
-  using SequenceOfWeakTimers = std::vector<detail::WeakTimerEntry>;
-  using TimersIterable = std::vector<detail::TimerEntry>;
+  using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
+  using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
 
-  using SequenceOfWeakClients = std::vector<detail::WeakClientEntry>;
-  using ClientsIterable = std::vector<detail::ClientEntry>;
+  using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
+  using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
 
-  using SequenceOfWeakServices = std::vector<detail::WeakServiceEntry>;
-  using ServicesIterable = std::vector<detail::ServiceEntry>;
+  using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
+  using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
 
-  using WeakWaitableEntry = detail::WeakWaitableEntry;
-  using WaitableEntry = detail::WaitableEntry;
+  class WaitableEntry
+  {
+public:
+    std::shared_ptr<rclcpp::Waitable> waitable;
+    std::shared_ptr<void> associated_entity;
 
+    /// Conversion constructor, which is intentionally not marked explicit.
+    WaitableEntry(
+      std::shared_ptr<rclcpp::Waitable> waitable_in = nullptr,
+      std::shared_ptr<void> associated_entity_in = nullptr) noexcept
+    : waitable(std::move(waitable_in)),
+      associated_entity(std::move(associated_entity_in))
+    {}
+
+    void
+    reset() noexcept
+    {
+      waitable.reset();
+      associated_entity.reset();
+    }
+  };
+  class WeakWaitableEntry
+  {
+public:
+    std::weak_ptr<rclcpp::Waitable> waitable;
+    std::weak_ptr<void> associated_entity;
+
+    explicit WeakWaitableEntry(
+      const std::shared_ptr<rclcpp::Waitable> & waitable_in,
+      const std::shared_ptr<void> & associated_entity_in) noexcept
+    : waitable(waitable_in),
+      associated_entity(associated_entity_in)
+    {}
+
+    explicit WeakWaitableEntry(const WaitableEntry & other)
+    : waitable(other.waitable),
+      associated_entity(other.associated_entity)
+    {}
+
+    std::shared_ptr<rclcpp::Waitable>
+    lock() const
+    {
+      return waitable.lock();
+    }
+
+    bool
+    expired() const noexcept
+    {
+      return waitable.expired();
+    }
+  };
   using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
   using WaitablesIterable = std::vector<WaitableEntry>;
 
@@ -93,9 +184,8 @@ protected:
       services,
       waitables,
       context),
-    // subscriptions_ is populated in the constructor dynamically, in order
-    // to respect the mask.
-    // shared_subscriptions_ is resized based on the result of subscriptions_.
+    subscriptions_(subscriptions.cbegin(), subscriptions.cend()),
+    shared_subscriptions_(subscriptions_.size()),
     guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()),
     shared_guard_conditions_(guard_conditions_.size()),
     timers_(timers.cbegin(), timers.cend()),
@@ -104,39 +194,9 @@ protected:
     shared_clients_(clients_.size()),
     services_(services.cbegin(), services.cend()),
     shared_services_(services_.size()),
-    waitables_(waitables.cbegin(), waitables.cend())
-    // shared_waitables_ is resized based on the result of waitables_ after
-    // waitables from subscriptions are added, if any.
-  {
-    // Ensure subscriptions are not being used by other wait sets and extract
-    // their waitables, if they have them.
-    std::vector<WaitableEntry> waitables_from_subscriptions;
-    for (auto & subscription_entry : subscriptions) {
-      std::vector<WaitableEntry> local_waitables_from_subscriptions = subscription_entry.manage();
-      if (subscription_entry.get_mask().include_subscription) {
-        subscriptions_.push_back(subscription_entry);
-      }
-    }
-    // Add subscription entries from subscriptions, if the subscription mask indicates.
-    std::copy_if(
-      subscriptions.cbegin(),
-      subscriptions.cend(),
-      std::back_inserter(subscriptions_),
-      [](const auto & subscription_entry) {
-        return subscription_entry.mask.include_subscription;
-      }
-    );
-    shared_subscriptions_.resize(subscriptions_.size());
-    // Add waitables from subscriptions, if the subscription mask indicates.
-    for (const auto & subscription_entry : subscriptions) {
-      if (subscription_entry.mask.include_events) {
-        for (const auto & event : subscription_entry.subscription->get_event_handlers()) {
-          waitables_.push_back({event, subscription_entry.subscription});
-        }
-      }
-    }
-    shared_waitables_.resize(waitables_.size());
-  }
+    waitables_(waitables.cbegin(), waitables.cend()),
+    shared_waitables_(waitables_.size())
+  {}
 
   ~DynamicStorage() = default;
 
@@ -183,7 +243,7 @@ protected:
     if (this->storage_has_entity(*subscription, subscriptions_)) {
       throw std::runtime_error("subscription already in wait set");
     }
-    WeakManagedSubscriptionEntry weak_entry{std::move(subscription), {}};
+    WeakSubscriptionEntry weak_entry{std::move(subscription), {}};
     subscriptions_.push_back(std::move(weak_entry));
     this->storage_flag_for_resize();
   }

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -20,13 +20,13 @@
 #include <utility>
 
 #include "rcl/wait.h"
+#include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/client.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
 #include "rclcpp/subscription_wait_set_mask.hpp"
@@ -657,7 +657,7 @@ public:
 
     // ensure the ownership of the entities in the wait set is shared for the duration of wait
     this->storage_acquire_ownerships();
-    RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();});
+    RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();});
 
     // this method comes from the SynchronizationPolicy
     return this->template sync_wait<WaitResult<WaitSetTemplate>>(

rclcpp/package.xml

@@ -2,15 +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>11.0.0</version>
+  <version>13.1.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
+  <buildtool_depend>python3</buildtool_depend>
 
   <build_depend>ament_index_cpp</build_depend>
   <build_depend>builtin_interfaces</build_depend>

rclcpp/resource/logging.hpp.em

@@ -149,7 +149,7 @@ def get_rclcpp_suffix_from_features(features):
 @[ if params]@
 @(''.join(['      ' + p + ', \\\n' for p in params if p != stream_arg]))@
 @[ end if]@
-      logger.get_name(), \
+      (logger).get_name(), \
 @[ if 'stream' not in feature_combination]@
       __VA_ARGS__); \
 @[ else]@

rclcpp/src/rclcpp/clock.cpp

@@ -14,11 +14,14 @@
 
 #include "rclcpp/clock.hpp"
 
+#include <condition_variable>
 #include <memory>
 #include <thread>
 
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/utilities.hpp"
 
+#include "rcpputils/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 
 namespace rclcpp
@@ -76,6 +79,99 @@ Clock::now()
   return now;
 }
 
+bool
+Clock::sleep_until(Time until, Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  const auto this_clock_type = get_clock_type();
+  if (until.get_clock_type() != this_clock_type) {
+    throw std::runtime_error("until's clock type does not match this clock's type");
+  }
+  bool time_source_changed = false;
+
+  std::condition_variable cv;
+
+  // Wake this thread if the context is shutdown
+  rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback(
+    [&cv]() {
+      cv.notify_one();
+    });
+  // No longer need the shutdown callback when this function exits
+  auto callback_remover = rcpputils::scope_exit(
+    [context, &shutdown_cb_handle]() {
+      context->remove_on_shutdown_callback(shutdown_cb_handle);
+    });
+
+  if (this_clock_type == RCL_STEADY_TIME) {
+    auto steady_time = std::chrono::steady_clock::time_point(
+      std::chrono::nanoseconds(until.nanoseconds()));
+
+    // loop over spurious wakeups but notice shutdown
+    std::unique_lock lock(impl_->clock_mutex_);
+    while (now() < until && context->is_valid()) {
+      cv.wait_until(lock, steady_time);
+    }
+  } else if (this_clock_type == RCL_SYSTEM_TIME) {
+    auto system_time = std::chrono::system_clock::time_point(
+      // Cast because system clock resolution is too big for nanoseconds on some systems
+      std::chrono::duration_cast<std::chrono::system_clock::duration>(
+        std::chrono::nanoseconds(until.nanoseconds())));
+
+    // loop over spurious wakeups but notice shutdown
+    std::unique_lock lock(impl_->clock_mutex_);
+    while (now() < until && context->is_valid()) {
+      cv.wait_until(lock, system_time);
+    }
+  } else if (this_clock_type == RCL_ROS_TIME) {
+    // Install jump handler for any amount of time change, for two purposes:
+    // - if ROS time is active, check if time reached on each new clock sample
+    // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep
+    rcl_jump_threshold_t threshold;
+    threshold.on_clock_change = true;
+    // 0 is disable, so -1 and 1 are smallest possible time changes
+    threshold.min_backward.nanoseconds = -1;
+    threshold.min_forward.nanoseconds = 1;
+    auto clock_handler = create_jump_callback(
+      nullptr,
+      [&cv, &time_source_changed](const rcl_time_jump_t & jump) {
+        if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) {
+          time_source_changed = true;
+        }
+        cv.notify_one();
+      },
+      threshold);
+
+    if (!ros_time_is_active()) {
+      auto system_time = std::chrono::system_clock::time_point(
+        // Cast because system clock resolution is too big for nanoseconds on some systems
+        std::chrono::duration_cast<std::chrono::system_clock::duration>(
+          std::chrono::nanoseconds(until.nanoseconds())));
+
+      // loop over spurious wakeups but notice shutdown or time source change
+      std::unique_lock lock(impl_->clock_mutex_);
+      while (now() < until && context->is_valid() && !time_source_changed) {
+        cv.wait_until(lock, system_time);
+      }
+    } else {
+      // RCL_ROS_TIME with ros_time_is_active.
+      // Just wait without "until" because installed
+      // jump callbacks wake the cv on every new sample.
+      std::unique_lock lock(impl_->clock_mutex_);
+      while (now() < until && context->is_valid() && !time_source_changed) {
+        cv.wait(lock);
+      }
+    }
+  }
+
+  if (!context->is_valid() || time_source_changed) {
+    return false;
+  }
+
+  return now() >= until;
+}
+
 bool
 Clock::ros_time_is_active()
 {

rclcpp/src/rclcpp/context.cpp

@@ -19,6 +19,7 @@
 #include <sstream>
 #include <string>
 #include <vector>
+#include <unordered_set>
 #include <utility>
 
 #include "rcl/init.h"
@@ -31,6 +32,8 @@
 #include "rcutils/error_handling.h"
 #include "rcutils/macros.h"
 
+#include "rmw/impl/cpp/demangle.hpp"
+
 #include "./logging_mutex.hpp"
 
 using rclcpp::Context;
@@ -305,6 +308,15 @@ Context::shutdown(const std::string & reason)
     // if it is not valid, then it cannot be shutdown
     return false;
   }
+
+  // call each pre-shutdown callback
+  {
+    std::lock_guard<std::mutex> lock{pre_shutdown_callbacks_mutex_};
+    for (const auto & callback : pre_shutdown_callbacks_) {
+      (*callback)();
+    }
+  }
+
   // rcl shutdown
   rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
   if (RCL_RET_OK != ret) {
@@ -353,36 +365,118 @@ Context::on_shutdown(OnShutdownCallback callback)
 rclcpp::OnShutdownCallbackHandle
 Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  auto callback_shared_ptr = std::make_shared<OnShutdownCallback>(callback);
-  {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    on_shutdown_callbacks_.emplace(callback_shared_ptr);
-  }
-
-  OnShutdownCallbackHandle callback_handle;
-  callback_handle.callback = callback_shared_ptr;
-  return callback_handle;
+  return add_shutdown_callback(ShutdownType::on_shutdown, callback);
 }
 
 bool
 Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+  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);
+}
+
+bool
+Context::remove_pre_shutdown_callback(
+  const PreShutdownCallbackHandle & callback_handle)
+{
+  return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
+}
+
+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;
+  }
+
+  ShutdownCallbackHandle callback_handle;
+  callback_handle.callback = callback_shared_ptr;
+  return callback_handle;
+}
+
+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();
   if (callback_shared_ptr == nullptr) {
     return false;
   }
-  return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1;
+  return callback_list_ptr->erase(callback_shared_ptr) == 1;
 }
 
 std::vector<rclcpp::Context::OnShutdownCallback>
 Context::get_on_shutdown_callbacks() const
 {
-  std::vector<OnShutdownCallback> callbacks;
+  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);
+}
+
+std::vector<rclcpp::Context::ShutdownCallback>
+Context::get_shutdown_callback(ShutdownType shutdown_type) const
+{
+  std::mutex * mutex_ptr = nullptr;
+  const 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::vector<rclcpp::Context::ShutdownCallback> callbacks;
   {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    for (auto & iter : on_shutdown_callbacks_) {
+    std::lock_guard<std::mutex> lock(*mutex_ptr);
+    for (auto & iter : *callback_list_ptr) {
       callbacks.emplace_back(*iter);
     }
   }
@@ -400,16 +494,15 @@ bool
 Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
 {
   std::chrono::nanoseconds time_left = nanoseconds;
-  {
-    std::unique_lock<std::mutex> lock(interrupt_mutex_);
-    auto start = std::chrono::steady_clock::now();
-    // this will release the lock while waiting
-    interrupt_condition_variable_.wait_for(lock, nanoseconds);
-    time_left -= std::chrono::steady_clock::now() - start;
-  }
-  if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) {
-    return sleep_for(time_left);
-  }
+  do {
+    {
+      std::unique_lock<std::mutex> lock(interrupt_mutex_);
+      auto start = std::chrono::steady_clock::now();
+      // this will release the lock while waiting
+      interrupt_condition_variable_.wait_for(lock, nanoseconds);
+      time_left -= std::chrono::steady_clock::now() - start;
+    }
+  } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());
   // Return true if the timeout elapsed successfully, otherwise false.
   return this->is_valid();
 }

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

@@ -1,104 +0,0 @@
-// Copyright 2021 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "rclcpp/detail/mutex_two_priorities.hpp"
-
-#include <mutex>
-
-namespace rclcpp
-{
-namespace detail
-{
-
-using LowPriorityLockable = MutexTwoPriorities::LowPriorityLockable;
-using HighPriorityLockable = MutexTwoPriorities::HighPriorityLockable;
-
-HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent)
-: parent_(parent)
-{}
-
-void
-HighPriorityLockable::lock()
-{
-  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
-  if (parent_.data_taken_) {
-    ++parent_.hp_waiting_count_;
-    while (parent_.data_taken_) {
-      parent_.hp_cv_.wait(guard);
-    }
-    --parent_.hp_waiting_count_;
-  }
-  parent_.data_taken_ = true;
-}
-
-void
-HighPriorityLockable::unlock()
-{
-  bool notify_lp{false};
-  {
-    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
-    parent_.data_taken_ = false;
-    notify_lp = 0u == parent_.hp_waiting_count_;
-  }
-  if (notify_lp) {
-    parent_.lp_cv_.notify_one();
-  } else {
-    parent_.hp_cv_.notify_one();
-  }
-}
-
-LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent)
-: parent_(parent)
-{}
-
-void
-LowPriorityLockable::lock()
-{
-  std::unique_lock<std::mutex> guard{parent_.cv_mutex_};
-  while (parent_.data_taken_ || parent_.hp_waiting_count_) {
-    parent_.lp_cv_.wait(guard);
-  }
-  parent_.data_taken_ = true;
-}
-
-void
-LowPriorityLockable::unlock()
-{
-  bool notify_lp{false};
-  {
-    std::lock_guard<std::mutex> guard{parent_.cv_mutex_};
-    parent_.data_taken_ = false;
-    notify_lp = 0u == parent_.hp_waiting_count_;
-  }
-  if (notify_lp) {
-    parent_.lp_cv_.notify_one();
-  } else {
-    parent_.hp_cv_.notify_one();
-  }
-}
-
-HighPriorityLockable
-MutexTwoPriorities::get_high_priority_lockable()
-{
-  return HighPriorityLockable{*this};
-}
-
-LowPriorityLockable
-MutexTwoPriorities::get_low_priority_lockable()
-{
-  return LowPriorityLockable{*this};
-}
-
-}  // namespace detail
-}  // namespace rclcpp

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp

@@ -19,8 +19,8 @@
 #include <vector>
 
 #include "rcl_yaml_param_parser/parser.h"
+#include "rcpputils/scope_exit.hpp"
 
-#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/parameter_map.hpp"
 
 std::map<std::string, rclcpp::ParameterValue>
@@ -47,7 +47,7 @@ rclcpp::detail::resolve_parameter_overrides(
       rclcpp::exceptions::throw_from_rcl_error(ret);
     }
     if (params) {
-      auto cleanup_params = make_scope_exit(
+      auto cleanup_params = rcpputils::make_scope_exit(
         [params]() {
           rcl_yaml_node_struct_fini(params);
         });

rclcpp/src/rclcpp/executor.cpp

@@ -22,17 +22,19 @@
 
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
+#include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
-#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
 
 #include "rcutils/logging_macros.h"
 
+#include "tracetools/tracetools.h"
+
 using namespace std::chrono_literals;
 
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -190,14 +192,12 @@ Executor::add_callback_groups_from_nodes_associated_to_executor()
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
-      auto group_ptrs = node->get_callback_groups();
-      std::for_each(
-        group_ptrs.begin(), group_ptrs.end(),
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+      node->for_each_callback_group(
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
         {
-          auto shared_group_ptr = group_ptr.lock();
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
+          if (
+            shared_group_ptr->automatically_add_to_executor_with_node() &&
+            !shared_group_ptr->get_associated_with_executor_atomic().load())
           {
             this->add_callback_group_to_map(
               shared_group_ptr,
@@ -273,18 +273,20 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
             "' has already been added to an executor.");
   }
   std::lock_guard<std::mutex> guard{mutex_};
-  for (auto & weak_group : node_ptr->get_callback_groups()) {
-    auto group_ptr = weak_group.lock();
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
-      group_ptr->automatically_add_to_executor_with_node())
+  node_ptr->for_each_callback_group(
+    [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr)
     {
-      this->add_callback_group_to_map(
-        group_ptr,
-        node_ptr,
-        weak_groups_to_nodes_associated_with_executor_,
-        notify);
-    }
-  }
+      if (!group_ptr->get_associated_with_executor_atomic().load() &&
+      group_ptr->automatically_add_to_executor_with_node())
+      {
+        this->add_callback_group_to_map(
+          group_ptr,
+          node_ptr,
+          weak_groups_to_nodes_associated_with_executor_,
+          notify);
+      }
+    });
+
   weak_nodes_.push_back(node_ptr);
 }
 
@@ -450,7 +452,7 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
   bool work_available = false;
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
@@ -484,7 +486,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_once() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
   spin_once_impl(timeout);
 }
 
@@ -515,9 +517,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     return;
   }
   if (any_exec.timer) {
+    TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.timer->get_timer_handle().get()));
     execute_timer(any_exec.timer);
   }
   if (any_exec.subscription) {
+    TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));
     execute_subscription(any_exec.subscription);
   }
   if (any_exec.service) {
@@ -678,6 +686,7 @@ Executor::execute_client(
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
+  TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
   {
     std::lock_guard<std::mutex> guard(mutex_);
 
@@ -827,6 +836,7 @@ Executor::get_next_ready_executable_from_map(
   const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
   weak_groups_to_nodes)
 {
+  TRACEPOINT(rclcpp_executor_get_next_ready);
   bool success = false;
   std::lock_guard<std::mutex> guard{mutex_};
   // Check the timers to see if there are any that are ready

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -19,10 +19,10 @@
 #include <memory>
 #include <vector>
 
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/scope_exit.hpp"
+#include "rcpputils/scope_exit.hpp"
+
+#include "rclcpp/utilities.hpp"
 
-using rclcpp::detail::MutexTwoPriorities;
 using rclcpp::executors::MultiThreadedExecutor;
 
 MultiThreadedExecutor::MultiThreadedExecutor(
@@ -48,12 +48,11 @@ MultiThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
-    auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
-    std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+    std::lock_guard wait_lock{wait_mutex_};
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
       auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
       threads.emplace_back(func);
@@ -78,26 +77,13 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
-      std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+      std::lock_guard wait_lock{wait_mutex_};
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
       if (!get_next_executable(any_exec, next_exec_timeout_)) {
         continue;
       }
-      if (any_exec.timer) {
-        // Guard against multiple threads getting the same timer.
-        if (scheduled_timers_.count(any_exec.timer) != 0) {
-          // Make sure that any_exec's callback group is reset before
-          // the lock is released.
-          if (any_exec.callback_group) {
-            any_exec.callback_group->can_be_taken_from().store(true);
-          }
-          continue;
-        }
-        scheduled_timers_.insert(any_exec.timer);
-      }
     }
     if (yield_before_execute_) {
       std::this_thread::yield();
@@ -105,14 +91,6 @@ MultiThreadedExecutor::run(size_t)
 
     execute_any_executable(any_exec);
 
-    if (any_exec.timer) {
-      auto high_priority_wait_mutex = wait_mutex_.get_high_priority_lockable();
-      std::lock_guard<MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
-      auto it = scheduled_timers_.find(any_exec.timer);
-      if (it != scheduled_timers_.end()) {
-        scheduled_timers_.erase(it);
-      }
-    }
     // Clear the callback_group to prevent the AnyExecutable destructor from
     // resetting the callback group `can_be_taken_from`
     any_exec.callback_group.reset();

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -12,9 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/any_executable.hpp"
-#include "rclcpp/scope_exit.hpp"
 
 using rclcpp::executors::SingleThreadedExecutor;
 
@@ -29,7 +30,7 @@ SingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_executable;
     if (get_next_executable(any_executable)) {

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -61,8 +61,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 void
 StaticExecutorEntitiesCollector::init(
   rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
-  rcl_guard_condition_t * executor_guard_condition)
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
 {
   // Empty initialize executable list
   exec_list_ = rclcpp::experimental::ExecutableList();
@@ -74,9 +73,6 @@ StaticExecutorEntitiesCollector::init(
   }
   memory_strategy_ = memory_strategy;
 
-  // Add executor's guard condition
-  memory_strategy_->add_guard_condition(executor_guard_condition);
-
   // Get memory strategy and executable list. Prepare wait_set_
   std::shared_ptr<void> shared_ptr;
   execute(shared_ptr);
@@ -108,6 +104,14 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
   fill_executable_list();
   // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize)
   prepare_wait_set();
+  // Add new nodes guard conditions to map
+  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+  for (const auto & weak_node : new_nodes_) {
+    if (auto node_ptr = weak_node.lock()) {
+      weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition();
+    }
+  }
+  new_nodes_.clear();
 }
 
 void
@@ -277,7 +281,8 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
 {
-  return weak_nodes_to_guard_conditions_.size();
+  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+  return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
 }
 
 bool
@@ -290,18 +295,20 @@ StaticExecutorEntitiesCollector::add_node(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
-  for (const auto & weak_group : node_ptr->get_callback_groups()) {
-    auto group_ptr = weak_group.lock();
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
-      group_ptr->automatically_add_to_executor_with_node())
+  node_ptr->for_each_callback_group(
+    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
     {
-      is_new_node = (add_callback_group(
+      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;
 }
@@ -327,7 +334,8 @@ StaticExecutorEntitiesCollector::add_callback_group(
     throw std::runtime_error("Callback group was already added to executor.");
   }
   if (is_new_node) {
-    weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition();
+    std::lock_guard<std::mutex> guard{new_nodes_mutex_};
+    new_nodes_.push_back(node_ptr);
     return true;
   }
   return false;
@@ -467,13 +475,10 @@ StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_ex
   for (const auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
-      auto group_ptrs = node->get_callback_groups();
-      std::for_each(
-        group_ptrs.begin(), group_ptrs.end(),
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+      node->for_each_callback_group(
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
         {
-          auto shared_group_ptr = group_ptr.lock();
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
+          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
           !shared_group_ptr->get_associated_with_executor_atomic().load())
           {
             add_callback_group(

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -16,9 +16,10 @@
 
 #include <chrono>
 #include <memory>
+#include <utility>
 #include <vector>
 
-#include "rclcpp/scope_exit.hpp"
+#include "rcpputils/scope_exit.hpp"
 
 using rclcpp::executors::StaticSingleThreadedExecutor;
 using rclcpp::experimental::ExecutableList;
@@ -43,11 +44,11 @@ StaticSingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
-  entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  entities_collector_->init(&wait_set_, memory_strategy_);
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work
@@ -81,7 +82,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
 {
   // Make sure the entities collector has been initialized
   if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    entities_collector_->init(&wait_set_, memory_strategy_);
   }
 
   auto start = std::chrono::steady_clock::now();
@@ -100,7 +101,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
 
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
@@ -118,7 +119,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 {
   // Make sure the entities collector has been initialized
   if (!entities_collector_->is_init()) {
-    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    entities_collector_->init(&wait_set_, memory_strategy_);
   }
 
   if (rclcpp::ok(context_) && spinning.load()) {
@@ -237,7 +238,9 @@ StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once)
   for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
     if (i < entities_collector_->get_number_of_timers()) {
       if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
-        execute_timer(entities_collector_->get_timer(i));
+        auto timer = entities_collector_->get_timer(i);
+        timer->call();
+        execute_timer(std::move(timer));
         if (spin_once) {
           return true;
         }

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -19,7 +19,6 @@
 #include "rcl/expand_topic_name.h"
 #include "rcl/validate_topic_name.h"
 #include "rclcpp/exceptions.hpp"
-#include "rclcpp/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 #include "rcutils/types/string_map.h"
 #include "rmw/error_handling.h"

rclcpp/src/rclcpp/init_options.cpp

@@ -43,7 +43,7 @@ InitOptions::InitOptions(const rcl_init_options_t & init_options)
 InitOptions::InitOptions(const InitOptions & other)
 : InitOptions(*other.get_rcl_init_options())
 {
-  shutdown_on_sigint = other.shutdown_on_sigint;
+  shutdown_on_signal = other.shutdown_on_signal;
   initialize_logging_ = other.initialize_logging_;
 }
 
@@ -70,7 +70,7 @@ InitOptions::operator=(const InitOptions & other)
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
     }
-    this->shutdown_on_sigint = other.shutdown_on_sigint;
+    this->shutdown_on_signal = other.shutdown_on_signal;
     this->initialize_logging_ = other.initialize_logging_;
   }
   return *this;

rclcpp/src/rclcpp/node.cpp

@@ -47,6 +47,9 @@ using rclcpp::Node;
 using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
+namespace
+{
+
 RCLCPP_LOCAL
 std::string
 extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
@@ -54,8 +57,14 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
   // Assumption is that the existing_sub_namespace does not need checking
   // because it would be checked already when it was set with this function.
 
-  // check if the new sub-namespace extension is absolute
-  if (extension.front() == '/') {
+  if (extension.empty()) {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "sub-nodes should not extend nodes by an empty sub-namespace",
+            0);
+  } else if (extension.front() == '/') {
+    // check if the new sub-namespace extension is absolute
     throw rclcpp::exceptions::NameValidationError(
             "sub_namespace",
             extension.c_str(),
@@ -70,7 +79,7 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
     new_sub_namespace = existing_sub_namespace + "/" + extension;
   }
 
-  // remove any trailing `/` so that new extensions do no result in `//`
+  // remove any trailing `/` so that new extensions do not result in `//`
   if (new_sub_namespace.back() == '/') {
     new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
   }
@@ -86,7 +95,11 @@ create_effective_namespace(const std::string & node_namespace, const std::string
   // and do not need trimming of `/` and other things, as they were validated
   // in other functions already.
 
-  if (node_namespace.back() == '/') {
+  // A node may not have a sub_namespace if it is no sub_node. In this case,
+  // just return the original namespace
+  if (sub_namespace.empty()) {
+    return node_namespace;
+  } else if (node_namespace.back() == '/') {
     // this is the special case where node_namespace is just `/`
     return node_namespace + sub_namespace;
   } else {
@@ -94,6 +107,8 @@ create_effective_namespace(const std::string & node_namespace, const std::string
   }
 }
 
+}  // namespace
+
 Node::Node(
   const std::string & node_name,
   const NodeOptions & options)
@@ -222,6 +237,8 @@ Node::Node(
   node_services_(other.node_services_),
   node_clock_(other.node_clock_),
   node_parameters_(other.node_parameters_),
+  node_time_source_(other.node_time_source_),
+  node_waitables_(other.node_waitables_),
   node_options_(other.node_options_),
   sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
   effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
@@ -482,10 +499,11 @@ Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_ma
   return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
 }
 
-const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-Node::get_callback_groups() const
+void
+Node::for_each_callback_group(
+  const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
 {
-  return node_base_->get_callback_groups();
+  node_base_->for_each_callback_group(func);
 }
 
 rclcpp::Event::SharedPtr

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -15,6 +15,7 @@
 #include <string>
 #include <limits>
 #include <memory>
+#include <utility>
 #include <vector>
 
 #include "rclcpp/node_interfaces/node_base.hpp"
@@ -31,6 +32,93 @@ using rclcpp::exceptions::throw_from_rcl_error;
 
 using rclcpp::node_interfaces::NodeBase;
 
+namespace
+{
+/// A class to manage the lifetime of a node handle and its context
+/**
+ * This class bundles together the lifetime of the rcl_node_t handle with the
+ * lifetime of the RCL context. This ensures that the context will remain alive
+ * for as long as the rcl_node_t handle is alive.
+ */
+class NodeHandleWithContext
+{
+public:
+  /// Make an instance of a NodeHandleWithContext
+  /**
+   * The make function returns a std::shared_ptr<rcl_node_t> which is actually
+   * an alias for a std::shared_ptr<NodeHandleWithContext>. There is no use for
+   * accessing a NodeHandleWithContext instance directly, because its only job
+   * is to couple together the lifetime of a context with the lifetime of a
+   * node handle that depends on it.
+   */
+  static std::shared_ptr<rcl_node_t>
+  make(
+    rclcpp::Context::SharedPtr context,
+    std::shared_ptr<std::recursive_mutex> logging_mutex,
+    rcl_node_t * node_handle)
+  {
+    auto aliased_ptr = std::make_shared<NodeHandleWithContext>(
+      NodeHandleWithContext(
+        std::move(context),
+        std::move(logging_mutex),
+        node_handle));
+
+    return std::shared_ptr<rcl_node_t>(std::move(aliased_ptr), node_handle);
+  }
+
+  // This class should not be copied. It should only exist in the
+  // std::shared_ptr that it was originally provided in.
+  NodeHandleWithContext(const NodeHandleWithContext &) = delete;
+  NodeHandleWithContext & operator=(const NodeHandleWithContext &) = delete;
+  NodeHandleWithContext & operator=(NodeHandleWithContext &&) = delete;
+
+  NodeHandleWithContext(NodeHandleWithContext && other)
+  : context_(std::move(other.context_)),
+    logging_mutex_(std::move(other.logging_mutex_)),
+    node_handle_(other.node_handle_)
+  {
+    other.node_handle_ = nullptr;
+  }
+
+  ~NodeHandleWithContext()
+  {
+    if (!node_handle_) {
+      // If the node_handle_ is null, then this object was moved-from. We don't
+      // need to do any cleanup.
+      return;
+    }
+
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+    // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
+    // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
+    // here directly.
+    if (rcl_node_fini(node_handle_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
+    }
+    delete node_handle_;
+  }
+
+private:
+  // The constructor is private because this class is only meant to be aliased
+  // into a std::shared_ptr<rcl_node_t> and should not be constructed any other
+  // way.
+  NodeHandleWithContext(
+    rclcpp::Context::SharedPtr context,
+    std::shared_ptr<std::recursive_mutex> logging_mutex,
+    rcl_node_t * node_handle)
+  : context_(std::move(context)),
+    logging_mutex_(std::move(logging_mutex)),
+    node_handle_(node_handle)
+  {}
+
+  rclcpp::Context::SharedPtr context_;
+  std::shared_ptr<std::recursive_mutex> logging_mutex_;
+  rcl_node_t * node_handle_;
+};
+}  // anonymous namespace
+
 NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
@@ -131,20 +219,7 @@ NodeBase::NodeBase(
     throw_from_rcl_error(ret, "failed to initialize rcl node");
   }
 
-  node_handle_.reset(
-    rcl_node.release(),
-    [logging_mutex](rcl_node_t * node) -> void {
-      std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
-      // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
-      // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
-      // here directly.
-      if (rcl_node_fini(node) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
-      }
-      delete node;
-    });
+  node_handle_ = NodeHandleWithContext::make(context_, logging_mutex, rcl_node.release());
 
   // Create the default callback group.
   using rclcpp::CallbackGroupType;
@@ -221,12 +296,10 @@ NodeBase::create_callback_group(
   rclcpp::CallbackGroupType group_type,
   bool automatically_add_to_executor_with_node)
 {
-  using rclcpp::CallbackGroup;
-  using rclcpp::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(
-    new CallbackGroup(
-      group_type,
-      automatically_add_to_executor_with_node));
+  auto group = std::make_shared<rclcpp::CallbackGroup>(
+    group_type,
+    automatically_add_to_executor_with_node);
+  std::lock_guard<std::mutex> lock(callback_groups_mutex_);
   callback_groups_.push_back(group);
   return group;
 }
@@ -240,20 +313,25 @@ NodeBase::get_default_callback_group()
 bool
 NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group)
 {
-  bool group_belongs_to_this_node = false;
+  std::lock_guard<std::mutex> lock(callback_groups_mutex_);
   for (auto & weak_group : this->callback_groups_) {
     auto cur_group = weak_group.lock();
     if (cur_group && (cur_group == group)) {
-      group_belongs_to_this_node = true;
+      return true;
     }
   }
-  return group_belongs_to_this_node;
+  return false;
 }
 
-const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-NodeBase::get_callback_groups() const
+void NodeBase::for_each_callback_group(const CallbackGroupFunction & func)
 {
-  return callback_groups_;
+  std::lock_guard<std::mutex> lock(callback_groups_mutex_);
+  for (rclcpp::CallbackGroup::WeakPtr & weak_group : this->callback_groups_) {
+    rclcpp::CallbackGroup::SharedPtr group = weak_group.lock();
+    if (group) {
+      func(group);
+    }
+  }
 }
 
 std::atomic_bool &

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -17,6 +17,7 @@
 #include <algorithm>
 #include <map>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -27,6 +28,7 @@
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rcpputils/scope_exit.hpp"
 
 using rclcpp::node_interfaces::NodeGraph;
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -178,6 +180,115 @@ NodeGraph::get_service_names_and_types_by_node(
   return services_and_types;
 }
 
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_client_names_and_types_by_node(
+  const std::string & node_name,
+  const std::string & namespace_) const
+{
+  rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();
+  auto service_names_and_types_finalizer = rcpputils::make_scope_exit(
+    [&service_names_and_types]() {
+      if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"), "could not destroy service names and types");
+      }
+    });
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_get_client_names_and_types_by_node(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    node_name.c_str(),
+    namespace_.c_str(),
+    &service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "failed to get service names and types by node");
+  }
+
+  std::map<std::string, std::vector<std::string>> services_and_types;
+  for (size_t i = 0; i < service_names_and_types.names.size; ++i) {
+    std::string service_name = service_names_and_types.names.data[i];
+    for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {
+      services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);
+    }
+  }
+
+  return services_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_publisher_names_and_types_by_node(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool no_demangle) const
+{
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
+  auto topic_names_and_types_finalizer = rcpputils::make_scope_exit(
+    [&topic_names_and_types]() {
+      if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"), "could not destroy topic names and types");
+      }
+    });
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    no_demangle,
+    node_name.c_str(),
+    namespace_.c_str(),
+    &topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "failed to get topic names and types by node");
+  }
+
+  std::map<std::string, std::vector<std::string>> topics_and_types;
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
+  }
+
+  return topics_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_subscriber_names_and_types_by_node(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool no_demangle) const
+{
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
+  auto topic_names_and_types_finalizer = rcpputils::make_scope_exit(
+    [&topic_names_and_types]() {
+      if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"), "could not destroy topic names and types");
+      }
+    });
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    no_demangle,
+    node_name.c_str(),
+    namespace_.c_str(),
+    &topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "failed to get topic names and types by node");
+  }
+
+  std::map<std::string, std::vector<std::string>> topics_and_types;
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
+  }
+
+  return topics_and_types;
+}
+
 std::vector<std::string>
 NodeGraph::get_node_names() const
 {
@@ -206,6 +317,81 @@ NodeGraph::get_node_names() const
   return nodes;
 }
 
+std::vector<std::tuple<std::string, std::string, std::string>>
+NodeGraph::get_node_names_with_enclaves() const
+{
+  rcutils_string_array_t node_names_c =
+    rcutils_get_zero_initialized_string_array();
+  rcutils_string_array_t node_namespaces_c =
+    rcutils_get_zero_initialized_string_array();
+  rcutils_string_array_t node_enclaves_c =
+    rcutils_get_zero_initialized_string_array();
+
+  auto allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_node_names_with_enclaves(
+    node_base_->get_rcl_node_handle(),
+    allocator,
+    &node_names_c,
+    &node_namespaces_c,
+    &node_enclaves_c);
+  if (ret != RCL_RET_OK) {
+    auto error_msg =
+      std::string("failed to get node names with enclaves: ") + rcl_get_error_string().str;
+    rcl_reset_error();
+    if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    if (rcutils_string_array_fini(&node_enclaves_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node enclaves, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    throw std::runtime_error(error_msg);
+  }
+
+  std::vector<std::tuple<std::string, std::string, std::string>> node_tuples;
+  for (size_t i = 0; i < node_names_c.size; ++i) {
+    if (node_names_c.data[i] && node_namespaces_c.data[i] && node_enclaves_c.data[i]) {
+      node_tuples.emplace_back(
+        std::make_tuple(node_names_c.data[i], node_namespaces_c.data[i], node_enclaves_c.data[i]));
+    }
+  }
+
+  std::string error("failed to finalize array");
+  rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);
+  if (ret_names != RCUTILS_RET_OK) {
+    error += std::string(", could not destroy node names, leaking memory: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+  }
+  rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);
+  if (ret_ns != RCUTILS_RET_OK) {
+    error += std::string(", could not destroy node namespaces, leaking memory: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+  }
+
+  rcl_ret_t ret_ecv = rcutils_string_array_fini(&node_enclaves_c);
+  if (ret_ecv != RCUTILS_RET_OK) {
+    error += std::string(", could not destroy node enclaves, leaking memory: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+  }
+
+  if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK || ret_ecv != RCUTILS_RET_OK) {
+    throw std::runtime_error(error);
+  }
+
+  return node_tuples;
+}
+
 std::vector<std::pair<std::string, std::string>>
 NodeGraph::get_node_names_and_namespaces() const
 {
@@ -237,7 +423,6 @@ NodeGraph::get_node_names_and_namespaces() const
     throw std::runtime_error(error_msg);
   }
 
-
   std::vector<std::pair<std::string, std::string>> node_names;
   node_names.reserve(node_names_c.size);
   for (size_t i = 0; i < node_names_c.size; ++i) {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -32,7 +32,6 @@
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/parameter_map.hpp"
-#include "rclcpp/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -14,9 +14,18 @@
 
 #include "rclcpp/node_interfaces/node_topics.hpp"
 
+#include <stdexcept>
 #include <string>
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/qos.hpp"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 

rclcpp/src/rclcpp/parameter_event_handler.cpp

@@ -29,10 +29,10 @@ ParameterEventCallbackHandle::SharedPtr
 ParameterEventHandler::add_parameter_event_callback(
   ParameterEventCallbackType callback)
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
   auto handle = std::make_shared<ParameterEventCallbackHandle>();
   handle->callback = callback;
-  event_callbacks_.emplace_front(handle);
+  callbacks_->event_callbacks_.emplace_front(handle);
 
   return handle;
 }
@@ -41,15 +41,15 @@ void
 ParameterEventHandler::remove_parameter_event_callback(
   ParameterEventCallbackHandle::SharedPtr callback_handle)
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
   auto it = std::find_if(
-    event_callbacks_.begin(),
-    event_callbacks_.end(),
+    callbacks_->event_callbacks_.begin(),
+    callbacks_->event_callbacks_.end(),
     [callback_handle](const auto & weak_handle) {
       return callback_handle.get() == weak_handle.lock().get();
     });
-  if (it != event_callbacks_.end()) {
-    event_callbacks_.erase(it);
+  if (it != callbacks_->event_callbacks_.end()) {
+    callbacks_->event_callbacks_.erase(it);
   } else {
     throw std::runtime_error("Callback doesn't exist");
   }
@@ -61,7 +61,7 @@ ParameterEventHandler::add_parameter_callback(
   ParameterCallbackType callback,
   const std::string & node_name)
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
   auto full_node_name = resolve_path(node_name);
 
   auto handle = std::make_shared<ParameterCallbackHandle>();
@@ -69,7 +69,7 @@ ParameterEventHandler::add_parameter_callback(
   handle->parameter_name = parameter_name;
   handle->node_name = full_node_name;
   // the last callback registered is executed first.
-  parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
+  callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
 
   return handle;
 }
@@ -78,9 +78,9 @@ void
 ParameterEventHandler::remove_parameter_callback(
   ParameterCallbackHandle::SharedPtr callback_handle)
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
   auto handle = callback_handle.get();
-  auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}];
+  auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}];
   auto it = std::find_if(
     container.begin(),
     container.end(),
@@ -90,7 +90,7 @@ ParameterEventHandler::remove_parameter_callback(
   if (it != container.end()) {
     container.erase(it);
     if (container.empty()) {
-      parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
+      callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
     }
   } else {
     throw std::runtime_error("Callback doesn't exist");
@@ -101,8 +101,8 @@ bool
 ParameterEventHandler::get_parameter_from_event(
   const rcl_interfaces::msg::ParameterEvent & event,
   rclcpp::Parameter & parameter,
-  const std::string parameter_name,
-  const std::string node_name)
+  const std::string & parameter_name,
+  const std::string & node_name)
 {
   if (event.node != node_name) {
     return false;
@@ -128,8 +128,8 @@ ParameterEventHandler::get_parameter_from_event(
 rclcpp::Parameter
 ParameterEventHandler::get_parameter_from_event(
   const rcl_interfaces::msg::ParameterEvent & event,
-  const std::string parameter_name,
-  const std::string node_name)
+  const std::string & parameter_name,
+  const std::string & node_name)
 {
   rclcpp::Parameter p;
   if (!get_parameter_from_event(event, p, parameter_name, node_name)) {
@@ -158,7 +158,7 @@ ParameterEventHandler::get_parameters_from_event(
 }
 
 void
-ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
+ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 

rclcpp/src/rclcpp/publisher_base.cpp

@@ -25,6 +25,7 @@
 #include <vector>
 
 #include "rcutils/logging_macros.h"
+#include "rmw/impl/cpp/demangle.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"

rclcpp/src/rclcpp/signal_handler.cpp

@@ -30,80 +30,131 @@
 #endif
 
 #include "rclcpp/logging.hpp"
+#include "rclcpp/utilities.hpp"
 #include "rcutils/strerror.h"
 #include "rmw/impl/cpp/demangle.hpp"
 
 using rclcpp::SignalHandler;
+using rclcpp::SignalHandlerOptions;
 
-// initialize static storage in SignalHandler class
-SignalHandler::signal_handler_type SignalHandler::old_signal_handler_;
-std::atomic_bool SignalHandler::signal_received_ = ATOMIC_VAR_INIT(false);
-std::atomic_bool SignalHandler::wait_for_signal_is_setup_ = ATOMIC_VAR_INIT(false);
-#if defined(_WIN32)
-HANDLE SignalHandler::signal_handler_sem_;
-#elif defined(__APPLE__)
-dispatch_semaphore_t SignalHandler::signal_handler_sem_;
-#else  // posix
-sem_t SignalHandler::signal_handler_sem_;
+SignalHandler::signal_handler_type
+SignalHandler::set_signal_handler(
+  int signal_value,
+  const SignalHandler::signal_handler_type & signal_handler)
+{
+  bool signal_handler_install_failed;
+  SignalHandler::signal_handler_type old_signal_handler;
+#if defined(RCLCPP_HAS_SIGACTION)
+  ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler);
+  signal_handler_install_failed = (ret == -1);
+#else
+  old_signal_handler = std::signal(signal_value, signal_handler);
+  signal_handler_install_failed = (old_signal_handler == SIG_ERR);
 #endif
+  if (signal_handler_install_failed) {
+    char error_string[1024];
+    rcutils_strerror(error_string, sizeof(error_string));
+    auto msg =
+      "Failed to set signal handler (" + std::to_string(errno) + "): " + error_string;
+    throw std::runtime_error(msg);
+  }
+  return old_signal_handler;
+}
 
-// The logger must be initialized before the local static variable signal_handler,
-// from the method get_global_signal_handler(), so that it is destructed after
-// it, because the destructor of SignalHandler uses this logger object.
-static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp");
+// Unfortunately macros (or duplicated code) are needed here,
+// as the signal handler must be a function pointer.
+#if defined(RCLCPP_HAS_SIGACTION)
+void
+SignalHandler::signal_handler(
+  int signum, siginfo_t * siginfo, void * context)
+{
+  RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum);
+  auto & instance = SignalHandler::get_global_signal_handler();
+
+  auto old_signal_handler = instance.get_old_signal_handler(signum);
+  if (old_signal_handler.sa_flags & SA_SIGINFO) {
+    if (old_signal_handler.sa_sigaction != NULL) {
+      old_signal_handler.sa_sigaction(signum, siginfo, context);
+    }
+  } else {
+    if (
+      old_signal_handler.sa_handler != NULL &&  /* Is set */
+      old_signal_handler.sa_handler != SIG_DFL &&  /* Is not default*/
+      old_signal_handler.sa_handler != SIG_IGN)  /* Is not ignored */
+    {
+      old_signal_handler.sa_handler(signum);
+    }
+  }
+  instance.signal_handler_common();
+}
+#else
+void
+SignalHandler::signal_handler(int signum)
+{
+  RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum);
+  auto & instance = SignalHandler::get_global_signal_handler();
+  auto old_signal_handler = instance.get_old_signal_handler(signum);
+  if (
+    SIG_ERR != old_signal_handler && SIG_IGN != old_signal_handler &&
+    SIG_DFL != old_signal_handler)
+  {
+    old_signal_handler(signum);
+  }
+  instance.signal_handler_common();
+}
+#endif
 
 rclcpp::Logger &
 SignalHandler::get_logger()
 {
-  return g_logger;
+  return SignalHandler::get_global_signal_handler().logger_;
 }
 
 SignalHandler &
 SignalHandler::get_global_signal_handler()
 {
-  // This is initialized after the g_logger static global, ensuring
-  // SignalHandler::get_logger() may be called from the destructor of
-  // SignalHandler, according to this:
-  //
-  //   Variables declared at block scope with the specifier static have static
-  //   storage duration but are initialized the first time control passes
-  //   through their declaration (unless their initialization is zero- or
-  //   constant-initialization, which can be performed before the block is
-  //   first entered). On all further calls, the declaration is skipped.
-  //
-  // -- https://en.cppreference.com/w/cpp/language/storage_duration#Static_local_variables
-  //
-  // Which is guaranteed to occur after static initialization for global (see:
-  // https://en.cppreference.com/w/cpp/language/initialization#Static_initialization),
-  // which is when g_logger will be initialized.
-  // And destruction will occur in the reverse order.
   static SignalHandler signal_handler;
   return signal_handler;
 }
 
 bool
-SignalHandler::install()
+SignalHandler::install(SignalHandlerOptions signal_handler_options)
 {
   std::lock_guard<std::mutex> lock(install_mutex_);
   bool already_installed = installed_.exchange(true);
   if (already_installed) {
     return false;
   }
+  if (signal_handler_options == SignalHandlerOptions::None) {
+    return true;
+  }
+  signal_handlers_options_ = signal_handler_options;
   try {
     setup_wait_for_signal();
     signal_received_.store(false);
 
-    SignalHandler::signal_handler_type signal_handler_argument;
+    SignalHandler::signal_handler_type handler_argument;
 #if defined(RCLCPP_HAS_SIGACTION)
-    memset(&signal_handler_argument, 0, sizeof(signal_handler_argument));
-    sigemptyset(&signal_handler_argument.sa_mask);
-    signal_handler_argument.sa_sigaction = signal_handler;
-    signal_handler_argument.sa_flags = SA_SIGINFO;
+    memset(&handler_argument, 0, sizeof(handler_argument));
+    sigemptyset(&handler_argument.sa_mask);
+    handler_argument.sa_sigaction = &this->signal_handler;
+    handler_argument.sa_flags = SA_SIGINFO;
 #else
-    signal_handler_argument = signal_handler;
+    handler_argument = &this->signal_handler;
 #endif
+    if (
+      signal_handler_options == SignalHandlerOptions::SigInt ||
+      signal_handler_options == SignalHandlerOptions::All)
+    {
+      old_sigint_handler_ = set_signal_handler(SIGINT, handler_argument);
+    }
 
-    old_signal_handler_ = SignalHandler::set_signal_handler(SIGINT, signal_handler_argument);
+    if (
+      signal_handler_options == SignalHandlerOptions::SigTerm ||
+      signal_handler_options == SignalHandlerOptions::All)
+    {
+      old_sigterm_handler_ = set_signal_handler(SIGTERM, handler_argument);
+    }
 
     signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this);
   } catch (...) {
@@ -125,7 +176,19 @@ SignalHandler::uninstall()
   try {
     // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall?
     //   I think we need to assert that we're the current signal handler, and mitigate if not.
-    set_signal_handler(SIGINT, old_signal_handler_);
+    if (
+      SignalHandlerOptions::SigInt == signal_handlers_options_ ||
+      SignalHandlerOptions::All == signal_handlers_options_)
+    {
+      set_signal_handler(SIGINT, old_sigint_handler_);
+    }
+    if (
+      SignalHandlerOptions::SigTerm == signal_handlers_options_ ||
+      SignalHandlerOptions::All == signal_handlers_options_)
+    {
+      set_signal_handler(SIGTERM, old_sigterm_handler_);
+    }
+    signal_handlers_options_ = SignalHandlerOptions::None;
     RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler");
     notify_signal_handler();
     signal_handler_thread_.join();
@@ -151,98 +214,57 @@ SignalHandler::~SignalHandler()
   } catch (const std::exception & exc) {
     RCLCPP_ERROR(
       get_logger(),
-      "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s",
+      "caught %s exception when uninstalling signal handlers in rclcpp::~SignalHandler: %s",
       rmw::impl::cpp::demangle(exc).c_str(), exc.what());
   } catch (...) {
     RCLCPP_ERROR(
       get_logger(),
-      "caught unknown exception when uninstalling the sigint handler in rclcpp::~SignalHandler");
+      "caught unknown exception when uninstalling signal handlers in rclcpp::~SignalHandler");
   }
 }
 
 SignalHandler::signal_handler_type
-SignalHandler::set_signal_handler(
-  int signal_value,
-  const SignalHandler::signal_handler_type & signal_handler)
+SignalHandler::get_old_signal_handler(int signum)
 {
-  bool signal_handler_install_failed;
-  SignalHandler::signal_handler_type old_signal_handler;
-#if defined(RCLCPP_HAS_SIGACTION)
-  ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler);
-  signal_handler_install_failed = (ret == -1);
-#else
-  old_signal_handler = std::signal(signal_value, signal_handler);
-  signal_handler_install_failed = (old_signal_handler == SIG_ERR);
-#endif
-  if (signal_handler_install_failed) {
-    char error_string[1024];
-    rcutils_strerror(error_string, sizeof(error_string));
-    auto msg =
-      "Failed to set SIGINT signal handler (" + std::to_string(errno) + "): " + error_string;
-    throw std::runtime_error(msg);
+  if (SIGINT == signum) {
+    return old_sigint_handler_;
+  } else if (SIGTERM == signum) {
+    return old_sigterm_handler_;
   }
-
-  return old_signal_handler;
+#if defined(RCLCPP_HAS_SIGACTION)
+  SignalHandler::signal_handler_type ret;
+  memset(&ret, 0, sizeof(ret));
+  sigemptyset(&ret.sa_mask);
+  ret.sa_handler = SIG_DFL;
+  return ret;
+#else
+  return SIG_DFL;
+#endif
 }
 
 void
 SignalHandler::signal_handler_common()
 {
-  signal_received_.store(true);
+  auto & instance = SignalHandler::get_global_signal_handler();
+  instance.signal_received_.store(true);
   RCLCPP_DEBUG(
     get_logger(),
-    "signal_handler(): SIGINT received, notifying deferred signal handler");
-  notify_signal_handler();
+    "signal_handler(): notifying deferred signal handler");
+  instance.notify_signal_handler();
 }
 
-#if defined(RCLCPP_HAS_SIGACTION)
-void
-SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context)
-{
-  RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value);
-
-  if (old_signal_handler_.sa_flags & SA_SIGINFO) {
-    if (old_signal_handler_.sa_sigaction != NULL) {
-      old_signal_handler_.sa_sigaction(signal_value, siginfo, context);
-    }
-  } else {
-    if (
-      old_signal_handler_.sa_handler != NULL &&  // Is set
-      old_signal_handler_.sa_handler != SIG_DFL &&  // Is not default
-      old_signal_handler_.sa_handler != SIG_IGN)  // Is not ignored
-    {
-      old_signal_handler_.sa_handler(signal_value);
-    }
-  }
-
-  signal_handler_common();
-}
-#else
-void
-SignalHandler::signal_handler(int signal_value)
-{
-  RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value);
-
-  if (old_signal_handler_) {
-    old_signal_handler_(signal_value);
-  }
-
-  signal_handler_common();
-}
-#endif
-
 void
 SignalHandler::deferred_signal_handler()
 {
   while (true) {
     if (signal_received_.exchange(false)) {
-      RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): SIGINT received, shutting down");
+      RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down");
       for (auto context_ptr : rclcpp::get_contexts()) {
-        if (context_ptr->get_init_options().shutdown_on_sigint) {
+        if (context_ptr->get_init_options().shutdown_on_signal) {
           RCLCPP_DEBUG(
             get_logger(),
             "deferred_signal_handler(): "
-            "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true",
+            "shutting down rclcpp::Context @ %p, because it had shutdown_on_signal == true",
             static_cast<void *>(context_ptr.get()));
           context_ptr->shutdown("signal handler");
         }
@@ -252,9 +274,11 @@ SignalHandler::deferred_signal_handler()
       RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled");
       break;
     }
-    RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): waiting for SIGINT or uninstall");
+    RCLCPP_DEBUG(
+      get_logger(), "deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall");
     wait_for_signal();
-    RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): woken up due to SIGINT or uninstall");
+    RCLCPP_DEBUG(
+      get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall");
   }
 }
 
@@ -356,3 +380,9 @@ SignalHandler::notify_signal_handler() noexcept
   }
 #endif
 }
+
+rclcpp::SignalHandlerOptions
+SignalHandler::get_current_signal_handler_options()
+{
+  return signal_handlers_options_;
+}

rclcpp/src/rclcpp/signal_handler.hpp

@@ -21,6 +21,7 @@
 #include <thread>
 
 #include "rclcpp/logging.hpp"
+#include "rclcpp/utilities.hpp"
 
 // includes for semaphore notification code
 #if defined(_WIN32)
@@ -39,14 +40,14 @@
 namespace rclcpp
 {
 
-/// Responsible for manaaging the SIGINT signal handling.
+/// Responsible for managing the SIGINT/SIGTERM signal handling.
 /**
  * This class is responsible for:
  *
- * - installing the signal handler for SIGINT
- * - uninstalling the signal handler for SIGINT
- * - creating a thread to execute "on sigint" work outside of the signal handler
- * - safely notifying the dedicated signal handling thread when receiving SIGINT
+ * - installing the signal handler for SIGINT/SIGTERM
+ * - uninstalling the signal handler for SIGINT/SIGTERM
+ * - creating a thread to execute "on signal" work outside of the signal handler
+ * - safely notifying the dedicated signal handling thread when receiving SIGINT/SIGTERM
  * - implementation of all of the signal handling work, like shutting down contexts
  *
  * \internal
@@ -64,15 +65,18 @@ public:
   rclcpp::Logger &
   get_logger();
 
-  /// Install the signal handler for SIGINT and start the dedicated signal handling thread.
+  /// Install the signal handler for SIGINT/SIGTERM and start the dedicated signal handling thread.
   /**
-   * Also stores the current signal handler to be called on SIGINT and to
+   * Also stores the current signal handler to be called on signal and to
    * restore when uninstalling this signal handler.
+   *
+   * \param signal_handler_options option to indicate which signal handlers should be installed.
    */
   bool
-  install();
+  install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
-  /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread.
+  /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated singal handling
+  /// thread.
   /**
    * Also restores the previous signal handler.
    */
@@ -83,26 +87,34 @@ public:
   bool
   is_installed();
 
+  /// Get the current signal handler options.
+  /**
+   * If no signal handler is installed, SignalHandlerOptions::None is returned.
+   */
+  rclcpp::SignalHandlerOptions
+  get_current_signal_handler_options();
+
 private:
-  SignalHandler() = default;
-
-  ~SignalHandler();
-
+  /// Signal handler type, platform dependent.
 #if defined(RCLCPP_HAS_SIGACTION)
   using signal_handler_type = struct sigaction;
 #else
   using signal_handler_type = void (*)(int);
 #endif
-  // POSIX signal handler structure storage for the existing signal handler.
-  static SignalHandler::signal_handler_type old_signal_handler_;
 
-  /// Set the signal handler function.
-  static
-  SignalHandler::signal_handler_type
-  set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler);
+
+  SignalHandler() = default;
+
+  ~SignalHandler();
+
+  SignalHandler(const SignalHandler &) = delete;
+  SignalHandler(SignalHandler &&) = delete;
+  SignalHandler &
+  operator=(const SignalHandler &) = delete;
+  SignalHandler &&
+  operator=(SignalHandler &&) = delete;
 
   /// Common signal handler code between sigaction and non-sigaction versions.
-  static
   void
   signal_handler_common();
 
@@ -127,7 +139,6 @@ private:
    * This must be called before wait_for_signal() or notify_signal_handler().
    * This is not thread-safe.
    */
-  static
   void
   setup_wait_for_signal();
 
@@ -137,7 +148,6 @@ private:
    *
    * This is not thread-safe.
    */
-  static
   void
   teardown_wait_for_signal() noexcept;
 
@@ -147,7 +157,6 @@ private:
    *
    * This is not thread-safe.
    */
-  static
   void
   wait_for_signal();
 
@@ -158,29 +167,45 @@ private:
    *
    * This is thread-safe.
    */
-  static
   void
   notify_signal_handler() noexcept;
 
+  static
+  signal_handler_type
+  set_signal_handler(
+    int signal_value,
+    const signal_handler_type & signal_handler);
+
+  signal_handler_type
+  get_old_signal_handler(int signum);
+
+  rclcpp::SignalHandlerOptions signal_handlers_options_ = rclcpp::SignalHandlerOptions::None;
+
+  signal_handler_type old_sigint_handler_;
+  signal_handler_type old_sigterm_handler_;
+
+  // logger instance
+  rclcpp::Logger logger_ = rclcpp::get_logger("rclcpp");
+
   // Whether or not a signal has been received.
-  static std::atomic_bool signal_received_;
+  std::atomic_bool signal_received_ = false;
   // A thread to which singal handling tasks are deferred.
   std::thread signal_handler_thread_;
 
   // A mutex used to synchronize the install() and uninstall() methods.
   std::mutex install_mutex_;
   // Whether or not the signal handler has been installed.
-  std::atomic_bool installed_{false};
+  std::atomic_bool installed_ = false;
 
   // Whether or not the semaphore for wait_for_signal is setup.
-  static std::atomic_bool wait_for_signal_is_setup_;
+  std::atomic_bool wait_for_signal_is_setup_;
   // Storage for the wait_for_signal semaphore.
 #if defined(_WIN32)
-  static HANDLE signal_handler_sem_;
+  HANDLE signal_handler_sem_;
 #elif defined(__APPLE__)
-  static dispatch_semaphore_t signal_handler_sem_;
+  dispatch_semaphore_t signal_handler_sem_;
 #else  // posix
-  static sem_t signal_handler_sem_;
+  sem_t signal_handler_sem_;
 #endif
 };
 

rclcpp/src/rclcpp/subscription_base.cpp

@@ -143,6 +143,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes
     &message_info_out.get_rmw_message_info(),
     nullptr  // rmw_subscription_allocation_t is unused here
   );
+  TRACEPOINT(rclcpp_take, static_cast<const void *>(message_out));
   if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
     return false;
   } else if (RCL_RET_OK != ret) {

rclcpp/src/rclcpp/time_source.cpp

@@ -33,29 +33,499 @@
 namespace rclcpp
 {
 
+class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
+{
+public:
+  ClocksState()
+  : logger_(rclcpp::get_logger("rclcpp"))
+  {
+  }
+
+  // An internal method to use in the clock callback that iterates and enables all clocks
+  void enable_ros_time()
+  {
+    if (ros_time_active_) {
+      // already enabled no-op
+      return;
+    }
+
+    // Local storage
+    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);
+    }
+  }
+
+  // An internal method to use in the clock callback that iterates and disables all clocks
+  void disable_ros_time()
+  {
+    if (!ros_time_active_) {
+      // already disabled no-op
+      return;
+    }
+
+    // Local storage
+    ros_time_active_ = false;
+
+    // Update all attached clocks
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
+      auto msg = std::make_shared<builtin_interfaces::msg::Time>();
+      set_clock(msg, false, *it);
+    }
+  }
+
+  // Check if ROS time is active
+  bool is_ros_time_active() const
+  {
+    return ros_time_active_;
+  }
+
+  // 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> 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);
+  }
+
+  // Detach a clock
+  void detachClock(rclcpp::Clock::SharedPtr clock)
+  {
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
+    if (result != associated_clocks_.end()) {
+      associated_clocks_.erase(result);
+    } else {
+      RCLCPP_ERROR(logger_, "failed to remove clock");
+    }
+  }
+
+  // Internal helper function used inside iterators
+  static void set_clock(
+    const builtin_interfaces::msg::Time::SharedPtr msg,
+    bool set_ros_time_enabled,
+    rclcpp::Clock::SharedPtr clock)
+  {
+    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");
+      }
+    } 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");
+    }
+  }
+
+  // Internal helper function
+  void set_all_clocks(
+    const builtin_interfaces::msg::Time::SharedPtr msg,
+    bool set_ros_time_enabled)
+  {
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
+      set_clock(msg, set_ros_time_enabled, *it);
+    }
+  }
+
+  // Cache the last clock message received
+  void cache_last_msg(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
+  {
+    last_msg_set_ = msg;
+  }
+
+private:
+  // Store (and update on node attach) logger for logging.
+  Logger logger_;
+
+  // A lock to protect iterating the associated_clocks_ field.
+  std::mutex clock_list_lock_;
+  // A vector to store references to associated clocks.
+  std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+
+  // Local storage of validity of ROS time
+  // 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_;
+};
+
+class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
+{
+public:
+  NodeState(std::weak_ptr<ClocksState> clocks_state, const rclcpp::QoS & qos, bool use_clock_thread)
+  : clocks_state_(std::move(clocks_state)),
+    use_clock_thread_(use_clock_thread),
+    logger_(rclcpp::get_logger("rclcpp")),
+    qos_(qos)
+  {
+  }
+
+  ~NodeState()
+  {
+    if (
+      node_base_ || node_topics_ || node_graph_ || node_services_ ||
+      node_logging_ || node_clock_ || node_parameters_)
+    {
+      detachNode();
+    }
+  }
+
+  // Check if a clock thread will be used
+  bool get_use_clock_thread()
+  {
+    return use_clock_thread_;
+  }
+
+  // Set whether a clock thread will be used
+  void set_use_clock_thread(bool use_clock_thread)
+  {
+    use_clock_thread_ = use_clock_thread;
+  }
+
+  // Check if the clock thread is joinable
+  bool clock_thread_is_joinable()
+  {
+    return clock_executor_thread_.joinable();
+  }
+
+  // Attach a node to this time source
+  void attachNode(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
+  {
+    node_base_ = node_base_interface;
+    node_topics_ = node_topics_interface;
+    node_graph_ = node_graph_interface;
+    node_services_ = node_services_interface;
+    node_logging_ = node_logging_interface;
+    node_clock_ = node_clock_interface;
+    node_parameters_ = node_parameters_interface;
+    // TODO(tfoote): Update QOS
+
+    logger_ = node_logging_->get_logger();
+
+    // Though this defaults to false, it can be overridden by initial parameter values for the
+    // node, which may be given by the user at the node's construction or even by command-line
+    // arguments.
+    rclcpp::ParameterValue use_sim_time_param;
+    const std::string use_sim_time_name = "use_sim_time";
+    if (!node_parameters_->has_parameter(use_sim_time_name)) {
+      use_sim_time_param = node_parameters_->declare_parameter(
+        use_sim_time_name,
+        rclcpp::ParameterValue(false),
+        rcl_interfaces::msg::ParameterDescriptor());
+    } else {
+      use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
+    }
+    if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
+      if (use_sim_time_param.get<bool>()) {
+        parameter_state_ = SET_TRUE;
+        // There should be no way to call this attachNode when the clocks_state_ pointer is not
+        // valid because it means the TimeSource is being destroyed
+        if (auto clocks_state_ptr = clocks_state_.lock()) {
+          clocks_state_ptr->enable_ros_time();
+          create_clock_sub();
+        }
+      }
+    } else {
+      RCLCPP_ERROR(
+        logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
+        rclcpp::to_string(use_sim_time_param.get_type()).c_str());
+    }
+    sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
+      [use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+        for (const auto & parameter : parameters) {
+          if (
+            parameter.get_name() == use_sim_time_name &&
+            parameter.get_type() != rclcpp::PARAMETER_BOOL)
+          {
+            result.successful = false;
+            result.reason = "'" + use_sim_time_name + "' must be a bool";
+            break;
+          }
+        }
+        return result;
+      });
+
+    // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
+    parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
+      node_topics_,
+      [state = std::weak_ptr<NodeState>(this->shared_from_this())](
+        std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
+        if (auto state_ptr = state.lock()) {
+          state_ptr->on_parameter_event(event);
+        }
+        // Do nothing if the pointer could not be locked because it means the TimeSource is now
+        // without an attached node
+      });
+  }
+
+  // Detach the attached node
+  void detachNode()
+  {
+    // There should be no way to call detachNode when the clocks_state_ pointer is not valid
+    // because it means the TimeSource is being destroyed
+    if (auto clocks_state_ptr = clocks_state_.lock()) {
+      clocks_state_ptr->disable_ros_time();
+    }
+    destroy_clock_sub();
+    parameter_subscription_.reset();
+    node_base_.reset();
+    node_topics_.reset();
+    node_graph_.reset();
+    node_services_.reset();
+    node_logging_.reset();
+    node_clock_.reset();
+    if (sim_time_cb_handler_ && node_parameters_) {
+      node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
+    }
+    sim_time_cb_handler_.reset();
+    node_parameters_.reset();
+  }
+
+private:
+  std::weak_ptr<ClocksState> clocks_state_;
+
+  // Dedicated thread for clock subscription.
+  bool use_clock_thread_;
+  std::thread clock_executor_thread_;
+
+  // Preserve the node reference
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  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::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  // Store (and update on node attach) logger for logging.
+  Logger logger_;
+
+  // QoS of the clock subscription.
+  rclcpp::QoS qos_;
+
+  // The subscription for the clock callback
+  using MessageT = rosgraph_msgs::msg::Clock;
+  using Alloc = std::allocator<void>;
+  using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
+  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
+  std::mutex clock_sub_lock_;
+  rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_;
+  std::promise<void> cancel_clock_executor_promise_;
+
+  // The clock callback itself
+  void clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
+  {
+    auto clocks_state_ptr = clocks_state_.lock();
+    if (!clocks_state_ptr) {
+      // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
+      // destroyed, so do nothing
+      return;
+    }
+    if (!clocks_state_ptr->is_ros_time_active() && SET_TRUE == this->parameter_state_) {
+      clocks_state_ptr->enable_ros_time();
+    }
+    // Cache the last message in case a new clock is attached.
+    clocks_state_ptr->cache_last_msg(msg);
+    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
+
+    if (SET_TRUE == this->parameter_state_) {
+      clocks_state_ptr->set_all_clocks(time_msg, true);
+    }
+  }
+
+  // Create the subscription for the clock topic
+  void create_clock_sub()
+  {
+    std::lock_guard<std::mutex> guard(clock_sub_lock_);
+    if (clock_subscription_) {
+      // Subscription already created.
+      return;
+    }
+
+    rclcpp::SubscriptionOptions options;
+    options.qos_overriding_options = rclcpp::QosOverridingOptions(
+      {
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability,
+      });
+
+    if (use_clock_thread_) {
+      clock_callback_group_ = node_base_->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive,
+        false
+      );
+      options.callback_group = clock_callback_group_;
+      rclcpp::ExecutorOptions exec_options;
+      exec_options.context = node_base_->get_context();
+      clock_executor_ =
+        std::make_shared<rclcpp::executors::SingleThreadedExecutor>(exec_options);
+      if (!clock_executor_thread_.joinable()) {
+        cancel_clock_executor_promise_ = std::promise<void>{};
+        clock_executor_thread_ = std::thread(
+          [this]() {
+            auto future = cancel_clock_executor_promise_.get_future();
+            clock_executor_->add_callback_group(clock_callback_group_, node_base_);
+            clock_executor_->spin_until_future_complete(future);
+          }
+        );
+      }
+    }
+
+    clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
+      node_parameters_,
+      node_topics_,
+      "/clock",
+      qos_,
+      [state = std::weak_ptr<NodeState>(this->shared_from_this())](
+        std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
+        if (auto state_ptr = state.lock()) {
+          state_ptr->clock_cb(msg);
+        }
+        // Do nothing if the pointer could not be locked because it means the TimeSource is now
+        // without an attached node
+      },
+      options
+    );
+  }
+
+  // Destroy the subscription for the clock topic
+  void destroy_clock_sub()
+  {
+    std::lock_guard<std::mutex> guard(clock_sub_lock_);
+    if (clock_executor_thread_.joinable()) {
+      cancel_clock_executor_promise_.set_value();
+      clock_executor_->cancel();
+      clock_executor_thread_.join();
+      clock_executor_->remove_callback_group(clock_callback_group_);
+    }
+    clock_subscription_.reset();
+  }
+
+  // Parameter Event subscription
+  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
+  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
+  std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
+
+  // Callback for parameter updates
+  void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
+  {
+    auto clocks_state_ptr = clocks_state_.lock();
+    if (!clocks_state_ptr) {
+      // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being
+      // destroyed, so do nothing
+      return;
+    }
+    // Filter out events on 'use_sim_time' parameter instances in other nodes.
+    if (event->node != node_base_->get_fully_qualified_name()) {
+      return;
+    }
+    // Filter for only 'use_sim_time' being added or changed.
+    rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"},
+      {rclcpp::ParameterEventsFilter::EventType::NEW,
+        rclcpp::ParameterEventsFilter::EventType::CHANGED});
+    for (auto & it : filter.get_events()) {
+      if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
+        RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
+        continue;
+      }
+      if (it.second->value.bool_value) {
+        parameter_state_ = SET_TRUE;
+        clocks_state_ptr->enable_ros_time();
+        create_clock_sub();
+      } else {
+        parameter_state_ = SET_FALSE;
+        clocks_state_ptr->disable_ros_time();
+        destroy_clock_sub();
+      }
+    }
+    // Handle the case that use_sim_time was deleted.
+    rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
+      {rclcpp::ParameterEventsFilter::EventType::DELETED});
+    for (auto & it : deleted.get_events()) {
+      (void) it;  // if there is a match it's already matched, don't bother reading it.
+      // If the parameter is deleted mark it as unset but dont' change state.
+      parameter_state_ = UNSET;
+    }
+  }
+
+  // An enum to hold the parameter state
+  enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE};
+  UseSimTimeParameterState parameter_state_;
+
+  // A handler for the use_sim_time parameter callback.
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
+};
+
 TimeSource::TimeSource(
   std::shared_ptr<rclcpp::Node> node,
   const rclcpp::QoS & qos,
   bool use_clock_thread)
-: use_clock_thread_(use_clock_thread),
-  logger_(rclcpp::get_logger("rclcpp")),
-  qos_(qos)
+: constructed_use_clock_thread_(use_clock_thread),
+  constructed_qos_(qos)
 {
-  this->attachNode(node);
+  clocks_state_ = std::make_shared<ClocksState>();
+  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
+  attachNode(node);
 }
 
 TimeSource::TimeSource(
   const rclcpp::QoS & qos,
   bool use_clock_thread)
-: use_clock_thread_(use_clock_thread),
-  logger_(rclcpp::get_logger("rclcpp")),
-  qos_(qos)
+: constructed_use_clock_thread_(use_clock_thread),
+  constructed_qos_(qos)
 {
+  clocks_state_ = std::make_shared<ClocksState>();
+  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
 }
 
 void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
 {
-  use_clock_thread_ = node->get_node_options().use_clock_thread();
+  node_state_->set_use_clock_thread(node->get_node_options().use_clock_thread());
   attachNode(
     node->get_node_base_interface(),
     node->get_node_topics_interface(),
@@ -75,298 +545,52 @@ void TimeSource::attachNode(
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
 {
-  node_base_ = node_base_interface;
-  node_topics_ = node_topics_interface;
-  node_graph_ = node_graph_interface;
-  node_services_ = node_services_interface;
-  node_logging_ = node_logging_interface;
-  node_clock_ = node_clock_interface;
-  node_parameters_ = node_parameters_interface;
-  // TODO(tfoote): Update QOS
-
-  logger_ = node_logging_->get_logger();
-
-  // Though this defaults to false, it can be overridden by initial parameter values for the node,
-  // which may be given by the user at the node's construction or even by command-line arguments.
-  rclcpp::ParameterValue use_sim_time_param;
-  const std::string use_sim_time_name = "use_sim_time";
-  if (!node_parameters_->has_parameter(use_sim_time_name)) {
-    use_sim_time_param = node_parameters_->declare_parameter(
-      use_sim_time_name,
-      rclcpp::ParameterValue(false),
-      rcl_interfaces::msg::ParameterDescriptor());
-  } else {
-    use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
-  }
-  if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
-    if (use_sim_time_param.get<bool>()) {
-      parameter_state_ = SET_TRUE;
-      enable_ros_time();
-      create_clock_sub();
-    }
-  } else {
-    RCLCPP_ERROR(
-      logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
-      rclcpp::to_string(use_sim_time_param.get_type()).c_str());
-  }
-  sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
-    [use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
-      rcl_interfaces::msg::SetParametersResult result;
-      result.successful = true;
-      for (const auto & parameter : parameters) {
-        if (
-          parameter.get_name() == use_sim_time_name &&
-          parameter.get_type() != rclcpp::PARAMETER_BOOL)
-        {
-          result.successful = false;
-          result.reason = "'" + use_sim_time_name + "' must be a bool";
-          break;
-        }
-      }
-      return result;
-    });
-
-  // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
-  parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
-    node_topics_,
-    std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1));
+  node_state_->attachNode(
+    std::move(node_base_interface),
+    std::move(node_topics_interface),
+    std::move(node_graph_interface),
+    std::move(node_services_interface),
+    std::move(node_logging_interface),
+    std::move(node_clock_interface),
+    std::move(node_parameters_interface));
 }
 
 void TimeSource::detachNode()
 {
-  this->ros_time_active_ = false;
-  destroy_clock_sub();
-  parameter_subscription_.reset();
-  node_base_.reset();
-  node_topics_.reset();
-  node_graph_.reset();
-  node_services_.reset();
-  node_logging_.reset();
-  node_clock_.reset();
-  if (sim_time_cb_handler_ && node_parameters_) {
-    node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
-  }
-  sim_time_cb_handler_.reset();
-  node_parameters_.reset();
-  disable_ros_time();
+  node_state_.reset();
+  node_state_ = std::make_shared<NodeState>(
+    clocks_state_->weak_from_this(),
+    constructed_qos_,
+    constructed_use_clock_thread_);
 }
 
 void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> 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> 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);
+  clocks_state_->attachClock(std::move(clock));
 }
 
 void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 {
-  std::lock_guard<std::mutex> guard(clock_list_lock_);
-  auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
-  if (result != associated_clocks_.end()) {
-    associated_clocks_.erase(result);
-  } else {
-    RCLCPP_ERROR(logger_, "failed to remove clock");
-  }
+  clocks_state_->detachClock(std::move(clock));
+}
+
+bool TimeSource::get_use_clock_thread()
+{
+  return node_state_->get_use_clock_thread();
+}
+
+void TimeSource::set_use_clock_thread(bool use_clock_thread)
+{
+  node_state_->set_use_clock_thread(use_clock_thread);
+}
+
+bool TimeSource::clock_thread_is_joinable()
+{
+  return node_state_->clock_thread_is_joinable();
 }
 
 TimeSource::~TimeSource()
 {
-  if (
-    node_base_ || node_topics_ || node_graph_ || node_services_ ||
-    node_logging_ || node_clock_ || node_parameters_)
-  {
-    this->detachNode();
-  }
-}
-
-void TimeSource::set_clock(
-  const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
-  std::shared_ptr<rclcpp::Clock> clock)
-{
-  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");
-    }
-  } 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");
-  }
-}
-
-void TimeSource::clock_cb(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
-{
-  if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) {
-    enable_ros_time();
-  }
-  // Cache the last message in case a new clock is attached.
-  last_msg_set_ = msg;
-  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
-
-  if (SET_TRUE == this->parameter_state_) {
-    std::lock_guard<std::mutex> guard(clock_list_lock_);
-    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-      set_clock(time_msg, true, *it);
-    }
-  }
-}
-
-void TimeSource::create_clock_sub()
-{
-  std::lock_guard<std::mutex> guard(clock_sub_lock_);
-  if (clock_subscription_) {
-    // Subscription already created.
-    return;
-  }
-
-  rclcpp::SubscriptionOptions options;
-  options.qos_overriding_options = rclcpp::QosOverridingOptions(
-    {
-      rclcpp::QosPolicyKind::Depth,
-      rclcpp::QosPolicyKind::Durability,
-      rclcpp::QosPolicyKind::History,
-      rclcpp::QosPolicyKind::Reliability,
-    });
-
-  if (use_clock_thread_) {
-    clock_callback_group_ = node_base_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive,
-      false
-    );
-    options.callback_group = clock_callback_group_;
-    rclcpp::ExecutorOptions exec_options;
-    exec_options.context = node_base_->get_context();
-    clock_executor_ =
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(exec_options);
-    if (!clock_executor_thread_.joinable()) {
-      cancel_clock_executor_promise_ = std::promise<void>{};
-      clock_executor_thread_ = std::thread(
-        [this]() {
-          auto future = cancel_clock_executor_promise_.get_future();
-          clock_executor_->add_callback_group(clock_callback_group_, node_base_);
-          clock_executor_->spin_until_future_complete(future);
-        }
-      );
-    }
-  }
-
-  clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
-    node_parameters_,
-    node_topics_,
-    "/clock",
-    rclcpp::QoS(KeepLast(1)).best_effort(),
-    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1),
-    options
-  );
-}
-
-void TimeSource::destroy_clock_sub()
-{
-  std::lock_guard<std::mutex> guard(clock_sub_lock_);
-  if (clock_executor_thread_.joinable()) {
-    cancel_clock_executor_promise_.set_value();
-    clock_executor_->cancel();
-    clock_executor_thread_.join();
-    clock_executor_->remove_callback_group(clock_callback_group_);
-  }
-  clock_subscription_.reset();
-}
-
-void TimeSource::on_parameter_event(
-  std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
-{
-  // Filter out events on 'use_sim_time' parameter instances in other nodes.
-  if (event->node != node_base_->get_fully_qualified_name()) {
-    return;
-  }
-  // Filter for only 'use_sim_time' being added or changed.
-  rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"},
-    {rclcpp::ParameterEventsFilter::EventType::NEW,
-      rclcpp::ParameterEventsFilter::EventType::CHANGED});
-  for (auto & it : filter.get_events()) {
-    if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
-      RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
-      continue;
-    }
-    if (it.second->value.bool_value) {
-      parameter_state_ = SET_TRUE;
-      enable_ros_time();
-      create_clock_sub();
-    } else {
-      parameter_state_ = SET_FALSE;
-      disable_ros_time();
-      destroy_clock_sub();
-    }
-  }
-  // Handle the case that use_sim_time was deleted.
-  rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
-    {rclcpp::ParameterEventsFilter::EventType::DELETED});
-  for (auto & it : deleted.get_events()) {
-    (void) it;  // if there is a match it's already matched, don't bother reading it.
-    // If the parameter is deleted mark it as unset but dont' change state.
-    parameter_state_ = UNSET;
-  }
-}
-
-void TimeSource::enable_ros_time()
-{
-  if (ros_time_active_) {
-    // already enabled no-op
-    return;
-  }
-
-  // Local storage
-  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);
-  }
-}
-
-void TimeSource::disable_ros_time()
-{
-  if (!ros_time_active_) {
-    // already disabled no-op
-    return;
-  }
-
-  // Local storage
-  ros_time_active_ = false;
-
-  // Update all attached clocks
-  std::lock_guard<std::mutex> guard(clock_list_lock_);
-  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    set_clock(msg, false, *it);
-  }
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/utilities.cpp

@@ -31,18 +31,22 @@ namespace rclcpp
 {
 
 void
-init(int argc, char const * const argv[], const InitOptions & init_options)
+init(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options,
+  SignalHandlerOptions signal_handler_options)
 {
   using rclcpp::contexts::get_global_default_context;
   get_global_default_context()->init(argc, argv, init_options);
   // Install the signal handlers.
-  install_signal_handlers();
+  install_signal_handlers(signal_handler_options);
 }
 
 bool
-install_signal_handlers()
+install_signal_handlers(SignalHandlerOptions signal_handler_options)
 {
-  return SignalHandler::get_global_signal_handler().install();
+  return SignalHandler::get_global_signal_handler().install(signal_handler_options);
 }
 
 bool
@@ -51,6 +55,13 @@ signal_handlers_installed()
   return SignalHandler::get_global_signal_handler().is_installed();
 }
 
+SignalHandlerOptions
+get_current_signal_handler_options()
+{
+  return SignalHandler::get_global_signal_handler().get_current_signal_handler_options();
+}
+
+
 bool
 uninstall_signal_handlers()
 {
@@ -156,12 +167,6 @@ ok(Context::SharedPtr context)
   return context->is_valid();
 }
 
-bool
-is_initialized(Context::SharedPtr context)
-{
-  return ok(context);
-}
-
 bool
 shutdown(Context::SharedPtr context, const std::string & reason)
 {

rclcpp/test/benchmark/benchmark_client.cpp

@@ -62,6 +62,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
     benchmark::DoNotOptimize(client);
     benchmark::ClobberMemory();
@@ -79,6 +80,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
     benchmark::DoNotOptimize(client);
     benchmark::ClobberMemory();
@@ -96,6 +98,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State &
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     state.PauseTiming();
     auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
     state.ResumeTiming();
@@ -109,6 +112,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State &
 BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) {
   int count = 0;
   for (auto _ : state) {
+    (void)_;
     state.PauseTiming();
     const std::string service_name = std::string("service_") + std::to_string(count++);
     // Create client before service so it has to 'discover' the service after construction
@@ -132,6 +136,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & s
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto future = client->async_send_request(shared_request);
     benchmark::DoNotOptimize(future);
     benchmark::ClobberMemory();
@@ -144,6 +149,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::S
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto future = client->async_send_request(shared_request);
     rclcpp::spin_until_future_complete(
       node->get_node_base_interface(), future, std::chrono::seconds(1));

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -20,7 +20,7 @@
 #include "performance_test_fixture/performance_test_fixture.hpp"
 
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp/scope_exit.hpp"
+#include "rcpputils/scope_exit.hpp"
 #include "test_msgs/msg/empty.hpp"
 
 using namespace std::chrono_literals;
@@ -78,6 +78,7 @@ BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
       publishers[i]->publish(empty_msgs);
@@ -104,6 +105,7 @@ BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark:
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
       publishers[i]->publish(empty_msgs);
@@ -142,6 +144,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benc
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -154,6 +157,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -165,6 +169,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(bench
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -176,6 +181,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -189,6 +195,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::StaticSingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -202,6 +209,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::StaticSingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -228,6 +236,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     // static_single_thread_executor has a special design. We need to add/remove the node each
     // time you call spin
     st.PauseTiming();
@@ -265,6 +274,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -294,6 +304,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -317,6 +328,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     auto ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -342,6 +354,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
       st.SkipWithError(rcutils_get_error_string().str);
@@ -366,7 +379,7 @@ BENCHMARK_F(
   if (ret != RCL_RET_OK) {
     st.SkipWithError(rcutils_get_error_string().str);
   }
-  RCLCPP_SCOPE_EXIT(
+  RCPPUTILS_SCOPE_EXIT(
   {
     rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
     if (ret != RCL_RET_OK) {
@@ -376,14 +389,14 @@ BENCHMARK_F(
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     std::shared_ptr<void> data = entities_collector_->take_data();
     entities_collector_->execute(data);
   }

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

@@ -26,6 +26,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state)
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     rclcpp::init(0, nullptr);
 
     state.PauseTiming();
@@ -43,6 +44,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state)
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     state.PauseTiming();
     rclcpp::init(0, nullptr);
     state.ResumeTiming();

rclcpp/test/benchmark/benchmark_node.cpp

@@ -44,6 +44,7 @@ BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     // Using pointer to separate construction and destruction in timing
     auto node = std::make_shared<rclcpp::Node>("node");
 #ifndef __clang_analyzer__
@@ -66,6 +67,7 @@ BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     // Using pointer to separate construction and destruction in timing
     state.PauseTiming();
     auto node = std::make_shared<rclcpp::Node>("node");

rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp

@@ -71,6 +71,7 @@ protected:
 BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     node->declare_parameter(param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor);
     node->undeclare_parameter(param3_name);
   }
@@ -79,6 +80,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & s
 BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     if (!node->has_parameter(param1_name)) {
       state.SkipWithError("Parameter was expected");
       break;
@@ -89,6 +91,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & s
 BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     if (node->has_parameter(param3_name)) {
       state.SkipWithError("Parameter was not expected");
       break;
@@ -112,6 +115,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State &
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->set_parameters(param_values2);
     node->set_parameters(param_values1);
   }
@@ -133,6 +137,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchma
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->set_parameters_atomically(param_values2);
     node->set_parameters_atomically(param_values1);
   }
@@ -164,6 +169,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->set_parameters(param_values2);
     node->set_parameters(param_values1);
   }
@@ -191,6 +197,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->set_parameters(param_values2);
     node->set_parameters(param_values1);
   }
@@ -212,6 +219,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->set_parameters(param_values2);
     node->set_parameters(param_values1);
   }
@@ -224,6 +232,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     node->get_parameter(param1_name, param1_value);
   }
 }
@@ -239,6 +248,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State &
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     param_list = node->list_parameters(prefixes, 10);
     if (param_list.names.size() != 2) {
       state.SkipWithError("Expected node names");
@@ -258,6 +268,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State
   reset_heap_counters();
 
   for (auto _ : state) {
+    (void)_;
     param_list = node->list_parameters(prefixes, 10);
     if (param_list.names.size() != 0) {
       state.SkipWithError("Expected no node names");

rclcpp/test/benchmark/benchmark_parameter_client.cpp

@@ -136,6 +136,7 @@ static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result
 BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     params_client.reset();
     params_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);
     if (!params_client->wait_for_service()) {
@@ -148,6 +149,7 @@ BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state
 BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     if (!params_client->has_parameter(param1_name)) {
       state.SkipWithError("Parameter was expected");
       break;
@@ -158,6 +160,7 @@ BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state)
 BENCHMARK_F(ParameterClientTest, has_parameter_miss)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     if (params_client->has_parameter(param3_name)) {
       state.SkipWithError("Parameter was not expected");
       break;
@@ -179,6 +182,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_bool)(benchmark::State & state)
   };
 
   for (auto _ : state) {
+    (void)_;
     std::vector<rcl_interfaces::msg::SetParametersResult> results =
       params_client->set_parameters(param_values2);
     if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
@@ -208,6 +212,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_atomically_bool)(benchmark::Stat
   };
 
   for (auto _ : state) {
+    (void)_;
     rcl_interfaces::msg::SetParametersResult result =
       params_client->set_parameters_atomically(param_values2);
     if (!result.successful) {
@@ -237,6 +242,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_string)(benchmark::State & state
   };
 
   for (auto _ : state) {
+    (void)_;
     std::vector<rcl_interfaces::msg::SetParametersResult> results =
       params_client->set_parameters(param_values2);
     if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
@@ -266,6 +272,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state)
   };
 
   for (auto _ : state) {
+    (void)_;
     std::vector<rcl_interfaces::msg::SetParametersResult> results =
       params_client->set_parameters(param_values2);
     if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
@@ -284,6 +291,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state)
 BENCHMARK_F(ParameterClientTest, get_parameters)(benchmark::State & state)
 {
   for (auto _ : state) {
+    (void)_;
     std::vector<rclcpp::Parameter> results = params_client->get_parameters({param1_name});
     if (results.size() != 1 || results[0].get_name() != param1_name) {
       state.SkipWithError("Got the wrong parameter(s)");
@@ -300,6 +308,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state)
   };
 
   for (auto _ : state) {
+    (void)_;
     rcl_interfaces::msg::ListParametersResult param_list =
       params_client->list_parameters(prefixes, 10);
     if (param_list.names.size() != 2) {
@@ -317,6 +326,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state)
   };
 
   for (auto _ : state) {
+    (void)_;
     rcl_interfaces::msg::ListParametersResult param_list =
       params_client->list_parameters(prefixes, 10);
     if (param_list.names.size() != 0) {

rclcpp/test/benchmark/benchmark_service.cpp

@@ -69,6 +69,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::Stat
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
     benchmark::DoNotOptimize(service);
     benchmark::ClobberMemory();
@@ -87,6 +88,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::Stat
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
     benchmark::DoNotOptimize(service);
     benchmark::ClobberMemory();
@@ -105,6 +107,7 @@ BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     state.PauseTiming();
     auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
     state.ResumeTiming();
@@ -123,6 +126,7 @@ BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & stat
 
   reset_heap_counters();
   for (auto _ : state) {
+    (void)_;
     state.PauseTiming();
     // Clear executor queue
     rclcpp::spin_some(node->get_node_base_interface());

rclcpp/test/rclcpp/CMakeLists.txt

@@ -12,6 +12,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   LIBRARY_NAME ${PROJECT_NAME}
   SKIP_INSTALL
 )
+# Need the target name to depend on generated interface libraries
+rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp")
 
 ament_add_gtest(
   test_allocator_common
@@ -374,8 +376,10 @@ if(TARGET test_publisher_with_type_adapter)
     "rosidl_typesupport_cpp"
     "test_msgs"
   )
-  rosidl_target_interfaces(test_publisher_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_publisher_with_type_adapter ${PROJECT_NAME} mimick)
+  target_link_libraries(test_publisher_with_type_adapter
+    ${PROJECT_NAME}
+    mimick
+    ${cpp_typesupport_target})
 endif()
 
 ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp
@@ -390,8 +394,10 @@ if(TARGET test_subscription_with_type_adapter)
     "rosidl_typesupport_cpp"
     "test_msgs"
   )
-  rosidl_target_interfaces(test_subscription_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_subscription_with_type_adapter ${PROJECT_NAME} mimick)
+  target_link_libraries(test_subscription_with_type_adapter
+    ${PROJECT_NAME}
+    mimick
+    ${cpp_typesupport_target})
 endif()
 
 ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp)
@@ -599,6 +605,13 @@ if(TARGET test_utilities)
   target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
 endif()
 
+ament_add_gtest(test_wait_for_message test_wait_for_message.cpp)
+if(TARGET test_wait_for_message)
+  ament_target_dependencies(test_wait_for_message
+    "test_msgs")
+  target_link_libraries(test_wait_for_message ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_interface_traits test_interface_traits.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_interface_traits)
@@ -673,8 +686,9 @@ if(TARGET test_subscription_topic_statistics)
     "rosidl_typesupport_cpp"
     "statistics_msgs"
     "test_msgs")
-  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
+  target_link_libraries(test_subscription_topic_statistics
+    ${PROJECT_NAME}
+    ${cpp_typesupport_target})
 endif()
 
 ament_add_gtest(test_subscription_options test_subscription_options.cpp)

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -20,6 +20,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
@@ -41,38 +43,38 @@ struct NumberOfEntities
 std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
 {
   auto number_of_entities = std::make_unique<NumberOfEntities>();
-  for (auto & weak_group : node->get_callback_groups()) {
-    auto group = weak_group.lock();
-    EXPECT_NE(nullptr, group);
-    if (!group || !group->can_be_taken_from().load()) {
-      return nullptr;
-    }
-    group->find_subscription_ptrs_if(
-      [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
-      {
-        number_of_entities->subscriptions++; return false;
-      });
-    group->find_timer_ptrs_if(
-      [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
-      {
-        number_of_entities->timers++; return false;
-      });
-    group->find_service_ptrs_if(
-      [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
-      {
-        number_of_entities->services++; return false;
-      });
-    group->find_client_ptrs_if(
-      [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
-      {
-        number_of_entities->clients++; return false;
-      });
-    group->find_waitable_ptrs_if(
-      [&number_of_entities](rclcpp::Waitable::SharedPtr &)
-      {
-        number_of_entities->waitables++; return false;
-      });
-  }
+  node->for_each_callback_group(
+    [&number_of_entities](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      if (!group->can_be_taken_from().load()) {
+        return;
+      }
+      group->find_subscription_ptrs_if(
+        [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
+        {
+          number_of_entities->subscriptions++; return false;
+        });
+      group->find_timer_ptrs_if(
+        [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
+        {
+          number_of_entities->timers++; return false;
+        });
+      group->find_service_ptrs_if(
+        [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
+        {
+          number_of_entities->services++; return false;
+        });
+      group->find_client_ptrs_if(
+        [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
+        {
+          number_of_entities->clients++; return false;
+        });
+      group->find_waitable_ptrs_if(
+        [&number_of_entities](rclcpp::Waitable::SharedPtr &)
+        {
+          number_of_entities->waitables++; return false;
+        });
+    });
 
   return number_of_entities;
 }
@@ -138,15 +140,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   // Check memory strategy is nullptr
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr;
   EXPECT_THROW(
-    entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition),
+    entities_collector_->init(&wait_set, memory_strategy),
     std::runtime_error);
 }
 
@@ -163,14 +164,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(
     expected_number_of_entities->subscriptions,
     entities_collector_->get_number_of_subscriptions());
@@ -183,7 +183,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
     entities_collector_->get_number_of_waitables());
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  entities_collector_->init(&wait_set, memory_strategy);
   EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
   EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
   EXPECT_EQ(0u, entities_collector_->get_number_of_services());
@@ -210,15 +210,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   // Expect weak_node pointers to be cleaned up and used
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
   EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
   EXPECT_EQ(0u, entities_collector_->get_number_of_services());
@@ -281,15 +280,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
 
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   EXPECT_EQ(
     1u + expected_number_of_entities->subscriptions,
@@ -308,7 +306,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
     entities_collector_->get_number_of_waitables());
 
   entities_collector_->remove_node(node->get_node_base_interface());
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  entities_collector_->init(&wait_set, memory_strategy);
   EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
   EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
   EXPECT_EQ(0u, entities_collector_->get_number_of_services());
@@ -361,14 +359,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
@@ -392,14 +389,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
@@ -430,14 +426,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
@@ -478,15 +473,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
 
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return(
@@ -511,14 +505,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   RCLCPP_EXPECT_THROW_EQ(
     entities_collector_->add_to_wait_set(nullptr),
@@ -539,11 +532,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group)
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -553,8 +545,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group)
 
   cb_group.reset();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
   ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
@@ -617,14 +609,13 @@ TEST_F(
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
 
   auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
   rclcpp::GuardCondition guard_condition(shared_context);
-  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
-  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy);
+  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
 
   cb_group->get_associated_with_executor_atomic().exchange(false);
   std::shared_ptr<void> data = entities_collector_->take_data();

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -122,6 +122,10 @@ TEST_F(TestNodeGraph, construct_from_node)
   auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 
+  auto names_namespaces_and_enclaves =
+    node_graph()->get_node_names_with_enclaves();
+  EXPECT_EQ(1u, names_namespaces_and_enclaves.size());
+
   EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic"));
   EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic"));
   EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition());
@@ -211,7 +215,7 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
     node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
 
   auto start = std::chrono::steady_clock::now();
-  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) {
     services_of_node1 =
       node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
     services_of_node2 =
@@ -225,6 +229,40 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
   EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end());
 }
 
+TEST_F(TestNodeGraph, get_client_names_and_types_by_node)
+{
+  auto client = node()->create_client<test_msgs::srv::Empty>("node1_service");
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
+
+  // rcl_get_client_names_and_types_by_node() expects the node to exist, otherwise it fails
+  EXPECT_THROW(
+    node_graph()->get_client_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    rclcpp::exceptions::RCLError);
+
+  // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph
+  // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
+  auto services_of_node1 =
+    node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace);
+  auto services_of_node2 =
+    node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) {
+    services_of_node1 =
+      node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace);
+    services_of_node2 =
+      node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace);
+    if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end());
+  EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end());
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors)
 {
   auto callback = [](
@@ -244,6 +282,21 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors)
       " service names and types, leaking memory: error not set"));
 }
 
+
+TEST_F(TestNodeGraph, get_client_names_and_types_by_node_rcl_errors)
+{
+  auto client = node()->create_client<test_msgs::srv::Empty>("node1_service");
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_client_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_client_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get service names and types by node: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error)
 {
   auto callback = [](
@@ -259,12 +312,163 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_e
     rclcpp::exceptions::RCLError);
 }
 
+TEST_F(TestNodeGraph, get_client_names_and_types_by_node_names_and_types_fini_error)
+{
+  auto client = node()->create_client<test_msgs::srv::Empty>("node1_service");
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(
+    node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace));
+}
+
+TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("node1_topic", publisher_qos);
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
+
+  // rcl_get_publisher_names_and_types_by_node() expects the node to exist, otherwise it fails
+  EXPECT_THROW(
+    node_graph()->get_publisher_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    rclcpp::exceptions::RCLError);
+
+  // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph
+  // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
+  auto topics_of_node1 =
+    node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace);
+  auto topics_of_node2 =
+    node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) {
+    topics_of_node1 =
+      node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace);
+    topics_of_node2 =
+      node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace);
+    if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end());
+  EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end());
+}
+
+TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node)
+{
+  const rclcpp::QoS subscriber_qos(10);
+  auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
+  auto subscription =
+    node()->create_subscription<test_msgs::msg::Empty>(
+    "node1_topic", subscriber_qos, std::move(callback));
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
+
+  // rcl_get_subscriber_names_and_types_by_node() expects the node to exist, otherwise it fails
+  EXPECT_THROW(
+    node_graph()->get_subscriber_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    rclcpp::exceptions::RCLError);
+
+  // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph
+  // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
+  auto topics_of_node1 =
+    node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace);
+  auto topics_of_node2 =
+    node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace);
+
+  auto start = std::chrono::steady_clock::now();
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) {
+    topics_of_node1 =
+      node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace);
+    topics_of_node2 =
+      node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace);
+    if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) {
+      break;
+    }
+  }
+
+  EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end());
+  EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end());
+}
+
+TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_rcl_errors)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publisher_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publisher_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get topic names and types by node: error not set"));
+}
+
+
+TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_rcl_errors)
+{
+  const rclcpp::QoS subscriber_qos(10);
+  auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
+  auto subscription =
+    node()->create_subscription<test_msgs::msg::Empty>(
+    "topic", subscriber_qos, std::move(callback));
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_subscriber_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_subscriber_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get topic names and types by node: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_names_and_types_fini_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(
+    node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace));
+}
+
+TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_names_and_types_fini_error)
+{
+  const rclcpp::QoS subscriber_qos(10);
+  auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
+  auto subscription =
+    node()->create_subscription<test_msgs::msg::Empty>(
+    "topic", subscriber_qos, std::move(callback));
+
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(
+    node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace));
+}
+
 TEST_F(TestNodeGraph, get_node_names_and_namespaces)
 {
   auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 }
 
+TEST_F(TestNodeGraph, get_node_names_with_enclaves)
+{
+  auto names_namespaces_and_enclaves =
+    node_graph()->get_node_names_with_enclaves();
+  EXPECT_EQ(1u, names_namespaces_and_enclaves.size());
+}
+
 TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors)
 {
   auto mock = mocking_utils::patch_and_return(
@@ -278,6 +482,20 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors)
       " error not set, failed also to cleanup node namespaces, leaking memory: error not set"));
 }
 
+TEST_F(TestNodeGraph, get_node_names_with_enclaves_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_node_names_with_enclaves, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_with_enclaves(),
+    std::runtime_error(
+      "failed to get node names with enclaves: error not set, failed also to cleanup node names, "
+      "leaking memory: error not set, failed also to cleanup node namespaces, leaking memory: "
+      "error not set, failed also to cleanup node enclaves, leaking memory: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors)
 {
   auto mock_names_fini = mocking_utils::patch_and_return(
@@ -287,6 +505,18 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors)
     std::runtime_error("could not destroy node names, could not destroy node namespaces"));
 }
 
+TEST_F(TestNodeGraph, get_node_names_with_enclaves_fini_errors)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_with_enclaves(),
+    std::runtime_error(
+      "failed to finalize array, could not destroy node names, leaking memory: error not set"
+      ", could not destroy node namespaces, leaking memory: error not set"
+      ", could not destroy node enclaves, leaking memory: error not set"));
+}
+
 TEST_F(TestNodeGraph, count_publishers_rcl_error)
 {
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR);

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -32,6 +32,7 @@ public:
   : TimerBase(node->get_clock(), std::chrono::nanoseconds(1),
       node->get_node_base_interface()->get_context()) {}
 
+  bool call() override {return true;}
   void execute_callback() override {}
   bool is_steady() override {return false;}
 };

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -21,8 +21,8 @@
 
 #include "gtest/gtest.h"
 
-#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
+#include "rcpputils/scope_exit.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
@@ -114,12 +114,11 @@ protected:
   {
     auto node = std::make_shared<rclcpp::Node>(name, "ns");
 
-    for (auto & group_weak_ptr : node->get_callback_groups()) {
-      auto group = group_weak_ptr.lock();
-      if (group) {
+    node->for_each_callback_group(
+      [](rclcpp::CallbackGroup::SharedPtr group)
+      {
         group->can_be_taken_from() = false;
-      }
-    }
+      });
     return node;
   }
 
@@ -201,15 +200,13 @@ protected:
     const RclWaitSetSizes & expected)
   {
     WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -234,15 +231,13 @@ protected:
     const RclWaitSetSizes & insufficient_capacities)
   {
     WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -268,7 +263,7 @@ protected:
              "Calling rcl_wait_set_init() with expected sufficient capacities failed";
     }
 
-    RCLCPP_SCOPE_EXIT(
+    RCPPUTILS_SCOPE_EXIT(
     {
       EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
     });
@@ -296,7 +291,7 @@ protected:
              "Calling rcl_wait_set_init() with expected insufficient capacities failed";
     }
 
-    RCLCPP_SCOPE_EXIT(
+    RCPPUTILS_SCOPE_EXIT(
     {
       EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity));
     });
@@ -316,26 +311,22 @@ protected:
   {
     auto basic_node = create_node_with_disabled_callback_groups("basic_node");
     WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = basic_node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &basic_node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    basic_node->for_each_callback_group(
+      [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             basic_node->get_node_base_interface()));
       });
-    auto callback_groups1 = node_with_entity1->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups1.begin(), callback_groups1.end(),
-      [&weak_groups_to_nodes,
-      &node_with_entity1](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_with_entity1->for_each_callback_group(
+      [node_with_entity1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node_with_entity1->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -348,26 +339,23 @@ protected:
 
     auto basic_node2 = std::make_shared<rclcpp::Node>("basic_node2", "ns");
     WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes;
-    auto callback_groups2 = basic_node2->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups2.begin(), callback_groups2.end(),
-      [&weak_groups_to_uncollected_nodes,
-      &basic_node2](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    basic_node2->for_each_callback_group(
+      [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_uncollected_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             basic_node2->get_node_base_interface()));
       });
-    auto callback_groups3 = node_with_entity2->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups3.begin(), callback_groups3.end(),
-      [&weak_groups_to_uncollected_nodes,
-      &node_with_entity2](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_with_entity2->for_each_callback_group(
+      [node_with_entity2,
+      &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_uncollected_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node_with_entity2->get_node_base_interface()));
       });
     rclcpp::AnyExecutable failed_result = get_next_entity_func(weak_groups_to_uncollected_nodes);
@@ -388,24 +376,22 @@ protected:
     auto basic_node_base = basic_node->get_node_base_interface();
     auto node_base = node_with_entity->get_node_base_interface();
     WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = basic_node_base->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &basic_node_base](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    basic_node_base->for_each_callback_group(
+      [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             basic_node_base));
       });
-    callback_groups = node_base->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node_base](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_base->for_each_callback_group(
+      [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node_base));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -447,14 +433,13 @@ private:
 TEST_F(TestAllocatorMemoryStrategy, construct_destruct) {
   auto basic_node = create_node_with_disabled_callback_groups("basic_node");
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = basic_node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &basic_node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  basic_node->for_each_callback_group(
+    [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
       weak_groups_to_nodes.insert(
         std::pair<rclcpp::CallbackGroup::WeakPtr,
         rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
           basic_node->get_node_base_interface()));
     });
   allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -546,14 +531,13 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) {
 TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) {
   auto node = create_node_with_subscription("subscription_node");
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  node->for_each_callback_group(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
       weak_groups_to_nodes.insert(
         std::pair<rclcpp::CallbackGroup::WeakPtr,
         rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
           node->get_node_base_interface()));
     });
   allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -594,7 +578,7 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) {
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options));
-  RCLCPP_SCOPE_EXIT(
+  RCPPUTILS_SCOPE_EXIT(
   {
     EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition));
   });
@@ -787,14 +771,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
       test_msgs::msg::Empty, decltype(subscription_callback)>(
       "topic", qos, std::move(subscription_callback), subscription_options);
 
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -821,14 +804,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
     auto service = node->create_service<test_msgs::srv::Empty>(
       "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group);
 
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -843,14 +825,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) {
   auto node = create_node_with_disabled_callback_groups("node");
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  node->for_each_callback_group(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
       weak_groups_to_nodes.insert(
         std::pair<rclcpp::CallbackGroup::WeakPtr,
         rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
           node->get_node_base_interface()));
     });
   // Force client to go out of scope and cleanup after collecting entities.
@@ -886,14 +867,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) {
       rclcpp::CallbackGroupType::MutuallyExclusive);
     auto timer = node->create_wall_timer(
       std::chrono::seconds(10), []() {}, callback_group);
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -913,14 +893,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) {
     auto callback_group =
       node->create_callback_group(
       rclcpp::CallbackGroupType::MutuallyExclusive);
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);

rclcpp/test/rclcpp/test_any_service_callback.cpp

@@ -22,6 +22,7 @@
 #include <utility>
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/service.hpp"
 #include "test_msgs/srv/empty.hpp"
 #include "test_msgs/srv/empty.h"
 
@@ -44,7 +45,7 @@ protected:
 
 TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) {
   EXPECT_THROW(
-    any_service_callback_.dispatch(request_header_, request_, response_),
+    any_service_callback_.dispatch(nullptr, request_header_, request_),
     std::runtime_error);
 }
 
@@ -57,7 +58,7 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) {
 
   any_service_callback_.set(callback);
   EXPECT_NO_THROW(
-    any_service_callback_.dispatch(request_header_, request_, response_));
+    EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
   EXPECT_EQ(callback_calls, 1);
 }
 
@@ -73,6 +74,36 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) {
 
   any_service_callback_.set(callback_with_header);
   EXPECT_NO_THROW(
-    any_service_callback_.dispatch(request_header_, request_, response_));
+    EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
+  EXPECT_EQ(callback_with_header_calls, 1);
+}
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) {
+  int callback_with_header_calls = 0;
+  auto callback_with_header =
+    [&callback_with_header_calls](const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<test_msgs::srv::Empty::Request>) {
+      callback_with_header_calls++;
+    };
+
+  any_service_callback_.set(callback_with_header);
+  EXPECT_NO_THROW(
+    EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
+  EXPECT_EQ(callback_with_header_calls, 1);
+}
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) {
+  int callback_with_header_calls = 0;
+  auto callback_with_header =
+    [&callback_with_header_calls](std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>>,
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<test_msgs::srv::Empty::Request>)
+    {
+      callback_with_header_calls++;
+    };
+
+  any_service_callback_.set(callback_with_header);
+  EXPECT_NO_THROW(
+    EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
   EXPECT_EQ(callback_with_header_calls, 1);
 }

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -19,6 +19,8 @@
 #include <string>
 #include <utility>
 
+// TODO(aprotyas): Figure out better way to suppress deprecation warnings.
+#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1
 #include "rclcpp/any_subscription_callback.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/msg/empty.h"
@@ -84,10 +86,12 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
     any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_),
     std::runtime_error);
   EXPECT_THROW(
-    any_subscription_callback_.dispatch_intra_process(msg_shared_ptr_, message_info_),
+    any_subscription_callback_.template dispatch_intra_process<test_msgs::msg::Empty>(
+      msg_shared_ptr_, message_info_),
     std::runtime_error);
   EXPECT_THROW(
-    any_subscription_callback_.dispatch_intra_process(get_unique_ptr_msg(), message_info_),
+    any_subscription_callback_.template dispatch_intra_process<test_msgs::msg::Empty>(
+      get_unique_ptr_msg(), message_info_),
     std::runtime_error);
 }
 
@@ -201,13 +205,15 @@ format_parameter_with_ta(const ::testing::TestParamInfo<DispatchTestsWithTA::Par
   /* Testing dispatch with shared_ptr<const MessageT> as input */ \
   TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \
     auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \
-    any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); \
+    any_subscription_callback_to_test.template dispatch_intra_process<test_msgs::msg::Empty>( \
+      msg_shared_ptr_, message_info_); \
   } \
  \
   /* Testing dispatch with unique_ptr<MessageT> as input */ \
   TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \
     auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \
-    any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); \
+    any_subscription_callback_to_test.template dispatch_intra_process<test_msgs::msg::Empty>( \
+      get_unique_ptr_msg(), message_info_); \
   }
 
 PARAMETERIZED_TESTS(DispatchTests)

rclcpp/test/rclcpp/test_client.cpp

@@ -216,7 +216,7 @@ protected:
         received_response = true;
       };
 
-    auto future = client->async_send_request(request, std::move(callback));
+    auto req_id = client->async_send_request(request, std::move(callback));
 
     auto start = std::chrono::steady_clock::now();
     while (!received_response &&
@@ -228,6 +228,9 @@ protected:
     if (!received_response) {
       return ::testing::AssertionFailure() << "Waiting for response timed out";
     }
+    if (client->remove_pending_request(req_id)) {
+      return ::testing::AssertionFailure() << "Should not be able to remove a finished request";
+    }
 
     return request_result;
   }
@@ -256,7 +259,7 @@ TEST_F(TestClientWithServer, async_send_request_callback_with_request) {
       EXPECT_NE(nullptr, request_response_pair.second);
       received_response = true;
     };
-  auto future = client->async_send_request(request, std::move(callback));
+  auto req_id = client->async_send_request(request, std::move(callback));
 
   auto start = std::chrono::steady_clock::now();
   while (!received_response &&
@@ -265,6 +268,15 @@ TEST_F(TestClientWithServer, async_send_request_callback_with_request) {
     rclcpp::spin_some(node);
   }
   EXPECT_TRUE(received_response);
+  EXPECT_FALSE(client->remove_pending_request(req_id));
+}
+
+TEST_F(TestClientWithServer, test_client_remove_pending_request) {
+  auto client = node->create_client<test_msgs::srv::Empty>("no_service_server_available_here");
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto future = client->async_send_request(request);
+
+  EXPECT_TRUE(client->remove_pending_request(future));
 }
 
 TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) {

rclcpp/test/rclcpp/test_generic_pubsub.cpp

@@ -187,7 +187,7 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos)
   auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos);
   auto subscription = node_->create_subscription<test_msgs::msg::Strings>(
     topic_name, qos,
-    [](std::shared_ptr<test_msgs::msg::Strings>/* message */) {});
+    [](std::shared_ptr<const test_msgs::msg::Strings>/* message */) {});
   auto connected = [publisher, subscription]() -> bool {
       return publisher->get_subscription_count() && subscription->get_publisher_count();
     };

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -116,6 +116,7 @@ public:
   {
     auto allocator = std::make_shared<Alloc>();
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    message_deleter_ = std::make_shared<MessageDeleter>();
   }
 
   // The following functions use the IntraProcessManager
@@ -123,6 +124,7 @@ public:
   void publish(MessageUniquePtr msg);
 
   std::shared_ptr<MessageAlloc> message_allocator_;
+  std::shared_ptr<MessageDeleter> message_deleter_;
 };
 
 }  // namespace mock
@@ -328,9 +330,15 @@ void Publisher<T, Alloc>::publish(MessageUniquePtr msg)
     throw std::runtime_error("cannot publish msg which is a null pointer");
   }
 
-  ipm->template do_intra_process_publish<T, Alloc>(
+  using MessageAllocTraits = allocator::AllocRebind<T, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, T>;
+
+  ipm->template do_intra_process_publish<T, MessageDeleter, T, Alloc>(
     intra_process_publisher_id_,
     std::move(msg),
+    *message_allocator_,
+    *message_deleter_,
     *message_allocator_);
 }
 

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/loaned_message.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -175,3 +176,19 @@ TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) {
     EXPECT_NO_THROW(loaned_message.reset());
   }
 }
+
+TEST_F(TestLoanedMessage, move_loaned_message) {
+  auto node = std::make_shared<rclcpp::Node>("loaned_message_test_node");
+  auto pub = node->create_publisher<MessageT>("loaned_message_test_topic", 1);
+
+  auto loaned_msg_to_move = pub->borrow_loaned_message();
+  // Force the move constructor to invoke
+  auto loaned_msg_moved_to = LoanedMessageT(std::move(loaned_msg_to_move));
+
+  ASSERT_TRUE(loaned_msg_moved_to.is_valid());
+  ASSERT_FALSE(loaned_msg_to_move.is_valid());
+
+  loaned_msg_moved_to.get().float32_value = 42.0f;
+  ASSERT_EQ(42.0f, loaned_msg_moved_to.get().float32_value);
+  SUCCEED();
+}

rclcpp/test/rclcpp/test_logging.cpp

@@ -229,6 +229,12 @@ TEST_F(TestLoggingMacros, test_throttle) {
   }
 }
 
+TEST_F(TestLoggingMacros, test_parameter_expression) {
+  RCLCPP_DEBUG_STREAM(*&g_logger, "message");
+  EXPECT_EQ(1u, g_log_calls);
+  EXPECT_EQ("message", g_last_log_event.message);
+}
+
 bool log_function(rclcpp::Logger logger)
 {
   RCLCPP_INFO(logger, "successful log");

rclcpp/test/rclcpp/test_memory_strategy.cpp

@@ -84,14 +84,13 @@ TEST_F(TestMemoryStrategy, get_subscription_by_handle) {
   rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -131,14 +130,13 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) {
   rclcpp::ServiceBase::SharedPtr found_service = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -184,14 +182,13 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) {
   rclcpp::ClientBase::SharedPtr found_client = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -232,14 +229,13 @@ TEST_F(TestMemoryStrategy, get_timer_by_handle) {
   rclcpp::TimerBase::SharedPtr found_timer = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -281,14 +277,13 @@ TEST_F(TestMemoryStrategy, get_node_by_group) {
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
     auto node_handle = node->get_node_base_interface();
-    auto callback_groups = node_handle->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node_handle](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_handle->for_each_callback_group(
+      [node_handle, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node_handle));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -325,14 +320,13 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) {
   rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -379,14 +373,13 @@ TEST_F(TestMemoryStrategy, get_group_by_service) {
   rclcpp::ServiceBase::SharedPtr service = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -424,14 +417,13 @@ TEST_F(TestMemoryStrategy, get_group_by_client) {
   rclcpp::ClientBase::SharedPtr client = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -464,14 +456,13 @@ TEST_F(TestMemoryStrategy, get_group_by_timer) {
   rclcpp::TimerBase::SharedPtr timer = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -504,14 +495,13 @@ TEST_F(TestMemoryStrategy, get_group_by_waitable) {
   rclcpp::Waitable::SharedPtr waitable = nullptr;
   {
     auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
         weak_groups_to_nodes.insert(
           std::pair<rclcpp::CallbackGroup::WeakPtr,
           rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
             node->get_node_base_interface()));
       });
     memory_strategy()->collect_entities(weak_groups_to_nodes);

rclcpp/test/rclcpp/test_node.cpp

@@ -16,6 +16,7 @@
 
 #include <chrono>
 #include <functional>
+#include <limits>
 #include <map>
 #include <memory>
 #include <string>
@@ -24,10 +25,10 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node.hpp"
-#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcpputils/filesystem_helper.hpp"
+#include "rcpputils/scope_exit.hpp"
 
 #include "rmw/validate_namespace.h"
 
@@ -96,11 +97,37 @@ TEST_F(TestNode, construction_and_destruction) {
   }
 }
 
+/*
+   Testing lifecycles of subscriptions and publishers after node dies
+ */
+TEST_F(TestNode, pub_and_sub_lifecycles) {
+  using test_msgs::msg::Empty;
+  rclcpp::Publisher<Empty>::SharedPtr pub;
+  rclcpp::Subscription<Empty>::SharedPtr sub;
+  const auto callback = [](Empty::ConstSharedPtr) {};
+
+  {
+    // Create the node and context in a nested scope so that their
+    // std::shared_ptrs expire before we use pub and sub
+    auto context = std::make_shared<rclcpp::Context>();
+    context->init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.context(context);
+
+    const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+    pub = node->create_publisher<Empty>("topic", 10);
+    sub = node->create_subscription<Empty>("topic", 10, callback);
+  }
+
+  pub->publish(Empty());
+}
+
 TEST_F(TestNode, get_name_and_namespace) {
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/ns", node->get_namespace());
+    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
   }
   {
@@ -115,30 +142,35 @@ TEST_F(TestNode, get_name_and_namespace) {
     auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/ns", node->get_namespace());
+    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/", node->get_namespace());
+    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/", node->get_namespace());
+    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/my/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/my/ns", node->get_namespace());
+    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "my/ns");
     EXPECT_STREQ("my_node", node->get_name());
     EXPECT_STREQ("/my/ns", node->get_namespace());
+    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
     EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
   }
   {
@@ -277,6 +309,13 @@ TEST_F(TestNode, subnode_construction_and_destruction) {
       auto subnode = node->create_sub_node("~sub_ns");
     }, rclcpp::exceptions::InvalidNamespaceError);
   }
+  {
+    ASSERT_THROW(
+    {
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("");
+    }, rclcpp::exceptions::NameValidationError);
+  }
 }
 
 TEST_F(TestNode, get_logger) {
@@ -409,7 +448,8 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});   // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
     EXPECT_THROW(
       {node->declare_parameter<std::string>(name, "not an int");},
       rclcpp::exceptions::InvalidParameterValueException);
@@ -656,7 +696,8 @@ TEST_F(TestNode, declare_parameter_with_overrides) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
     EXPECT_THROW(
       {node->declare_parameter<int>(name, 43);},
       rclcpp::exceptions::InvalidParameterValueException);
@@ -783,7 +824,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
     EXPECT_THROW(
       {node->declare_parameters<std::string>("", {{name, "not an int"}});},
       rclcpp::exceptions::InvalidParameterValueException);
@@ -1011,7 +1053,8 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
 
     EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful);
   }
@@ -1519,7 +1562,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
 
     auto rets = node->set_parameters(
     {
@@ -1711,7 +1755,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
 
     auto ret = node->set_parameters_atomically(
     {
@@ -1837,7 +1882,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) {
         return result;
       };
     auto handler = node->add_on_set_parameters_callback(on_set_parameters);
-    RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());});    // always reset
+    RCPPUTILS_SCOPE_EXIT(
+      {node->remove_on_set_parameters_callback(handler.get());});  // always reset
 
     auto ret = node->set_parameters_atomically(
     {
@@ -2515,6 +2561,59 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) {
   }
 }
 
+// test declare parameter with int, int64_t, float and double vector
+TEST_F(TestNode, declare_parameter_with_vector) {
+  auto node = std::make_shared<rclcpp::Node>(
+    "test_declare_parameter_with_vector"_unq,
+    rclcpp::NodeOptions().allow_undeclared_parameters(true));
+  {
+    // declare parameter and then get types to check
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto name4 = "parameter"_unq;
+
+    node->declare_parameter(name1, std::vector<int>{});
+    node->declare_parameter(name2, std::vector<int64_t>{});
+    node->declare_parameter(name3, std::vector<float>{});
+    node->declare_parameter(name4, std::vector<double>{});
+
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_TRUE(node->has_parameter(name3));
+    EXPECT_TRUE(node->has_parameter(name4));
+
+    auto results = node->get_parameter_types({name1, name2, name3, name4});
+    EXPECT_EQ(results.size(), 4u);
+    EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY);
+    EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY);
+    EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY);
+    EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY);
+  }
+  {
+    // declare parameter and then get values to check
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto name4 = "parameter"_unq;
+
+    int64_t bigger_than_int = INT64_MAX - 42;
+    double bigger_than_float = std::numeric_limits<double>::max() - 42;
+    node->declare_parameter(name1, std::vector<int>{1, 2});
+    node->declare_parameter(name2, std::vector<int64_t>{3, bigger_than_int});
+    node->declare_parameter(name3, std::vector<float>{1.5f, 2.8f});
+    node->declare_parameter(name4, std::vector<double>{3.0, bigger_than_float});
+
+    std::vector<rclcpp::Parameter> expected = {
+      {name1, std::vector<int>{1, 2}},
+      {name2, std::vector<int64_t>{3, bigger_than_int}},
+      {name3, std::vector<float>{1.5f, 2.8f}},
+      {name4, std::vector<double>{3.0, bigger_than_float}},
+    };
+    EXPECT_EQ(node->get_parameters({name1, name2, name3, name4}), expected);
+  }
+}
+
 void expect_qos_profile_eq(
   const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher)
 {
@@ -2679,13 +2778,33 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {
 
 TEST_F(TestNode, callback_groups) {
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  size_t num_callback_groups_in_basic_node = node->get_callback_groups().size();
+  size_t num_callback_groups_in_basic_node = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups_in_basic_node](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups_in_basic_node++;
+    });
 
   auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_EQ(1u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+  size_t num_callback_groups = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups++;
+    });
+  EXPECT_EQ(1u + num_callback_groups_in_basic_node, num_callback_groups);
 
   auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  EXPECT_EQ(2u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+  size_t num_callback_groups2 = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups2](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups2++;
+    });
+  EXPECT_EQ(2u + num_callback_groups_in_basic_node, num_callback_groups2);
 }
 
 // This is tested more thoroughly in node_interfaces/test_node_graph

rclcpp/test/rclcpp/test_parameter_event_handler.cpp

@@ -29,17 +29,17 @@ public:
 
   void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event)
   {
-    event_callback(*event);
+    callbacks_->event_callback(*event);
   }
 
   size_t num_event_callbacks()
   {
-    return event_callbacks_.size();
+    return callbacks_->event_callbacks_.size();
   }
 
   size_t num_parameter_callbacks()
   {
-    return parameter_callbacks_.size();
+    return callbacks_->parameter_callbacks_.size();
   }
 };