13.1.0

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

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

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)
   {

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

@@ -75,7 +75,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 +93,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.
    *
@@ -268,7 +268,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.

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/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/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_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

@@ -438,10 +438,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) {

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/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,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>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>
@@ -11,6 +11,7 @@
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>python3</buildtool_depend>
 
   <build_depend>ament_index_cpp</build_depend>
   <build_depend>builtin_interfaces</build_depend>

rclcpp/src/rclcpp/context.cpp

@@ -31,6 +31,8 @@
 #include "rcutils/error_handling.h"
 #include "rcutils/macros.h"
 
+#include "rmw/impl/cpp/demangle.hpp"
+
 #include "./logging_mutex.hpp"
 
 using rclcpp::Context;
@@ -400,16 +402,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

@@ -221,12 +221,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 +238,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/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",
+      rclcpp::QoS(KeepLast(1)).best_effort(),
+      [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_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;
@@ -366,7 +366,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,10 +376,9 @@ 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();
 

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"

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_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"
 
@@ -101,6 +102,7 @@ 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 +117,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 +284,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 +423,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 +671,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 +799,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 +1028,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 +1537,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 +1730,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 +1857,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 +2536,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 +2753,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();
   }
 };
 

rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp

@@ -191,7 +191,7 @@ TEST_F(
 
   auto callback =
     [message_data, &is_received](
-    const rclcpp::msg::String::SharedPtr msg,
+    const rclcpp::msg::String::ConstSharedPtr msg,
     const rclcpp::MessageInfo & message_info
     ) -> void
     {

rclcpp/test/rclcpp/test_subscription.cpp

@@ -510,7 +510,7 @@ INSTANTIATE_TEST_SUITE_P(
 TEST_F(TestSubscription, get_network_flow_endpoints_errors) {
   initialize();
   const rclcpp::QoS subscription_qos(1);
-  auto subscription_callback = [](test_msgs::msg::Empty::SharedPtr msg) {
+  auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) {
       (void)msg;
     };
   auto subscription = node->create_subscription<test_msgs::msg::Empty>(

rclcpp/test/rclcpp/test_time_source.cpp

@@ -530,12 +530,12 @@ public:
 
   bool GetUseClockThreadOption()
   {
-    return this->use_clock_thread_;
+    return this->get_use_clock_thread();
   }
 
   bool IsClockThreadJoinable()
   {
-    return this->clock_executor_thread_.joinable();
+    return this->clock_thread_is_joinable();
   }
 };
 

rclcpp/test/rclcpp/test_utilities.cpp

@@ -24,7 +24,6 @@
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/utilities.hpp"
-#include "rclcpp/scope_exit.hpp"
 
 #include "../mocking_utils/patch.hpp"
 #include "../utils/rclcpp_gtest_macros.hpp"

rclcpp/test/rclcpp/test_wait_for_message.cpp

@@ -0,0 +1,74 @@
+// 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 <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/wait_for_message.hpp"
+
+#include "test_msgs/msg/strings.hpp"
+#include "test_msgs/message_fixtures.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestUtilities, wait_for_message) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node");
+
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
+
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  for (auto i = 0u; i < 10 && received == false; ++i) {
+    pub->publish(*get_messages_strings()[0]);
+    std::this_thread::sleep_for(1s);
+  }
+  ASSERT_TRUE(received);
+  EXPECT_EQ(out, *get_messages_strings()[0]);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestUtilities, wait_for_message_indefinitely) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node2");
+
+  using MsgT = test_msgs::msg::Strings;
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic" /*, -1 */);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  rclcpp::shutdown();
+
+  ASSERT_FALSE(received);
+}

rclcpp/test/rclcpp/test_wait_set.cpp

@@ -14,7 +14,6 @@
 
 #include <gtest/gtest.h>
 
-#include <chrono>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -343,9 +342,6 @@ TEST_F(TestWaitSet, get_result_from_wait_result) {
   EXPECT_EQ(&wait_set, &const_result.get_wait_set());
 }
 
-/*
- * Fail to get wait_set from result when timeout (not ready).
- */
 TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
   rclcpp::WaitSet wait_set;
   auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
@@ -363,97 +359,3 @@ TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
     const_result.get_wait_set(),
     std::runtime_error("cannot access wait set when the result was not ready"));
 }
-
-/*
- * Fail to add item to wait set, which was added in the constructor of another wait set.
- *
- * Also ensure items in destroyed wait sets can be added to new wait sets afterwards.
- */
-TEST_F(TestWaitSet, add_entity_in_constructor_and_dynamically_fails) {
-  auto node = std::make_shared<rclcpp::Node>("add_entity_in_constructor_and_dynamically_fails");
-
-  rclcpp::SubscriptionOptions subscription_options;
-  subscription_options.event_callbacks.deadline_callback = [](auto) {};
-  subscription_options.event_callbacks.liveliness_callback = [](auto) {};
-  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
-  auto sub = node->create_subscription<test_msgs::msg::BasicTypes>(
-    "~/test",
-    1,
-    do_nothing,
-    subscription_options);
-
-  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
-
-  auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {});
-
-  auto client = node->create_client<rcl_interfaces::srv::ListParameters>("~/empty");
-
-  auto srv_callback =
-    [](
-      const std::shared_ptr<rmw_request_id_t>,
-      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request>,
-      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>
-    ) {};
-  auto service =
-    node->create_service<rcl_interfaces::srv::ListParameters>("~/test", srv_callback);
-
-  {
-    // Add all entities via constructor.
-    rclcpp::WaitSet wait_set(
-      {{sub}},
-      {guard_condition},
-      {timer},
-      {client},
-      {service}
-    );
-
-    // Expect all cannot be added to another wait set.
-    rclcpp::WaitSet other_wait_set;
-    EXPECT_THROW({
-      other_wait_set.add_subscription(sub);
-    }, std::runtime_error);
-    EXPECT_THROW({
-      other_wait_set.add_guard_condition(guard_condition);
-    }, std::runtime_error);
-    EXPECT_THROW({
-      other_wait_set.add_timer(timer);
-    }, std::runtime_error);
-    EXPECT_THROW({
-      other_wait_set.add_client(client);
-    }, std::runtime_error);
-    EXPECT_THROW({
-      other_wait_set.add_service(service);
-    }, std::runtime_error);
-  }
-}
-
-/*
- * Ensure removing a subscription added via construction is removable with all mask items.
- *
- * This covers a case that had a bug in which the adding of items via construction was naive and
- * did not behave as-if items were being added via `add_*` type methods, specifically for
- * subscriptions, which had the mask logic.
- */
-TEST_F(TestWaitSet, remove_subscription_which_was_added_via_construction) {
-  auto node =
-    std::make_shared<rclcpp::Node>("remove_subscription_which_was_added_via_construction");
-
-  rclcpp::SubscriptionOptions subscription_options;
-  subscription_options.event_callbacks.deadline_callback = [](auto) {};
-  subscription_options.event_callbacks.liveliness_callback = [](auto) {};
-  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
-  auto sub = node->create_subscription<test_msgs::msg::BasicTypes>(
-    "~/test",
-    1,
-    do_nothing,
-    subscription_options);
-
-  rclcpp::WaitSet wait_set({{sub}});
-
-  wait_set.remove_subscription(sub);
-}
-
-/*
-More test ideas:
-  - add to dynamic wait set, let go out of scope, destroy wait set (weak ptr to objects prevent exchange_in_use_by_wait_set_state(false), should be ok though)
- */

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,27 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+13.1.0 (2021-10-18)
+-------------------
+* Deprecate the `void shared_ptr<MessageT>` subscription callback signatures (`#1713 <https://github.com/ros2/rclcpp/issues/1713>`_)
+* Contributors: Abrar Rahman Protyasha, Tomoya Fujita
+
+13.0.0 (2021-08-23)
+-------------------
+* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 <https://github.com/ros2/rclcpp/issues/1727>`_)
+* Contributors: Christophe Bedard
+
+12.0.0 (2021-07-26)
+-------------------
+
+11.2.0 (2021-07-21)
+-------------------
+
+11.1.0 (2021-07-13)
+-------------------
+* Fixed occasionally missing goal result caused by race condition (`#1677 <https://github.com/ros2/rclcpp/issues/1677>`_)
+* Contributors: Kaven Yau
+
 11.0.0 (2021-05-18)
 -------------------
 * Bump the benchmark timeout for benchmark_action_client (`#1671 <https://github.com/ros2/rclcpp/issues/1671>`_)

rclcpp_action/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(action_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_action REQUIRED)
+find_package(rcpputils REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
 
 # Default to C++14
@@ -39,6 +40,7 @@ ament_target_dependencies(${PROJECT_NAME}
   "action_msgs"
   "rcl_action"
   "rclcpp"
+  "rcpputils"
   "rosidl_runtime_c"
 )
 
@@ -94,6 +96,7 @@ if(BUILD_TESTING)
   ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180)
   if(TARGET test_server)
     ament_target_dependencies(test_server
+      "rcpputils"
       "rcutils"
       "test_msgs"
     )

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>11.0.0</version>
+  <version>13.1.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
@@ -19,6 +19,7 @@
   <depend>action_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rcl_action</depend>
+  <depend>rcpputils</depend>
 
   <depend>ament_cmake</depend>
 

rclcpp_action/src/server.cpp

@@ -15,10 +15,11 @@
 #include <rcl_action/action_server.h>
 #include <rcl_action/wait.h>
 
+#include <rcpputils/scope_exit.hpp>
+
 #include <action_msgs/msg/goal_status_array.hpp>
 #include <action_msgs/srv/cancel_goal.hpp>
 #include <rclcpp/exceptions.hpp>
-#include <rclcpp/scope_exit.hpp>
 #include <rclcpp_action/server.hpp>
 
 #include <memory>
@@ -416,7 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  RCLCPP_SCOPE_EXIT(
+  RCPPUTILS_SCOPE_EXIT(
   {
     ret = rcl_action_cancel_response_fini(&cancel_response);
     if (RCL_RET_OK != ret) {
@@ -581,7 +582,7 @@ ServerBase::publish_status()
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  RCLCPP_SCOPE_EXIT(
+  RCPPUTILS_SCOPE_EXIT(
   {
     ret = rcl_action_goal_status_array_fini(&c_status_array);
     if (RCL_RET_OK != ret) {

rclcpp_action/test/test_server.cpp

@@ -15,7 +15,7 @@
 #include <rclcpp/exceptions.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <rclcpp/scope_exit.hpp>
+#include <rcpputils/scope_exit.hpp>
 #include <test_msgs/action/fibonacci.hpp>
 
 #include <gtest/gtest.h>
@@ -205,6 +205,27 @@ TEST_F(TestServer, construction_and_destruction_wait_set_error)
   }, rclcpp::exceptions::RCLError);
 }
 
+TEST_F(TestServer, construction_and_destruction_sub_node)
+{
+  auto parent_node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
+  auto sub_node = parent_node->create_sub_node("construct_sub_node");
+
+  ASSERT_NO_THROW(
+  {
+    using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+    auto as = rclcpp_action::create_server<Fibonacci>(
+      sub_node, "fibonacci",
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    (void)as;
+  });
+}
+
 TEST_F(TestServer, handle_goal_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
@@ -485,10 +506,10 @@ TEST_F(TestServer, publish_status_accepted)
   (void)as;
 
   // Subscribe to status messages
-  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  std::vector<action_msgs::msg::GoalStatusArray::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
     "fibonacci/_action/status", 10,
-    [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list)
     {
       received_msgs.push_back(list);
     });
@@ -548,10 +569,10 @@ TEST_F(TestServer, publish_status_canceling)
   (void)as;
 
   // Subscribe to status messages
-  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  std::vector<action_msgs::msg::GoalStatusArray::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
     "fibonacci/_action/status", 10,
-    [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list)
     {
       received_msgs.push_back(list);
     });
@@ -605,10 +626,10 @@ TEST_F(TestServer, publish_status_canceled)
   (void)as;
 
   // Subscribe to status messages
-  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  std::vector<action_msgs::msg::GoalStatusArray::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
     "fibonacci/_action/status", 10,
-    [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list)
     {
       received_msgs.push_back(list);
     });
@@ -664,10 +685,10 @@ TEST_F(TestServer, publish_status_succeeded)
   (void)as;
 
   // Subscribe to status messages
-  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  std::vector<action_msgs::msg::GoalStatusArray::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
     "fibonacci/_action/status", 10,
-    [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list)
     {
       received_msgs.push_back(list);
     });
@@ -721,10 +742,10 @@ TEST_F(TestServer, publish_status_aborted)
   (void)as;
 
   // Subscribe to status messages
-  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  std::vector<action_msgs::msg::GoalStatusArray::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
     "fibonacci/_action/status", 10,
-    [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list)
     {
       received_msgs.push_back(list);
     });
@@ -779,9 +800,9 @@ TEST_F(TestServer, publish_feedback)
 
   // Subscribe to feedback messages
   using FeedbackT = Fibonacci::Impl::FeedbackMessage;
-  std::vector<FeedbackT::SharedPtr> received_msgs;
+  std::vector<FeedbackT::ConstSharedPtr> received_msgs;
   auto subscriber = node->create_subscription<FeedbackT>(
-    "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg)
+    "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::ConstSharedPtr msg)
     {
       received_msgs.push_back(msg);
     });
@@ -1082,7 +1103,7 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) {
   ASSERT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator));
-  RCLCPP_SCOPE_EXIT(
+  RCPPUTILS_SCOPE_EXIT(
   {
     EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
   });

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,27 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+13.1.0 (2021-10-18)
+-------------------
+
+13.0.0 (2021-08-23)
+-------------------
+* Update client API to be able to remove pending requests. (`#1734 <https://github.com/ros2/rclcpp/issues/1734>`_)
+* Contributors: Ivan Santiago Paunovic
+
+12.0.0 (2021-07-26)
+-------------------
+
+11.2.0 (2021-07-21)
+-------------------
+* Deprecate method names that use CamelCase in rclcpp_components. (`#1716 <https://github.com/ros2/rclcpp/issues/1716>`_)
+* Contributors: Rebecca Butler
+
+11.1.0 (2021-07-13)
+-------------------
+* Added a hook to generate node options in ComponentManager (`#1702 <https://github.com/ros2/rclcpp/issues/1702>`_)
+* Contributors: Rebecca Butler
+
 11.0.0 (2021-05-18)
 -------------------
 

rclcpp_components/include/rclcpp_components/component_manager.hpp

@@ -109,7 +109,7 @@ public:
   virtual ~ComponentManager();
 
   /// Return a list of valid loadable components in a given package.
-  /*
+  /**
    * \param package_name name of the package
    * \param resource_index name of the executable
    * \throws ComponentManagerException if the resource was not found or a invalid resource entry
@@ -122,7 +122,7 @@ public:
     const std::string & resource_index = "rclcpp_components") const;
 
   /// Instantiate a component from a dynamic library.
-  /*
+  /**
    * \param resource a component resource (class name + library path)
    * \return a NodeFactory interface
    */
@@ -131,8 +131,17 @@ public:
   create_component_factory(const ComponentResource & resource);
 
 protected:
+  /// Create node options for loaded component
+  /**
+   * \param request information with the node to load
+   * \return node options
+   */
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual rclcpp::NodeOptions
+  create_node_options(const std::shared_ptr<LoadNode::Request> request);
+
   /// Service callback to load a new node in the component
-  /*
+  /**
    * This function allows to add parameters, remap rules, a specific node, name a namespace
    * and/or additional arguments.
    *
@@ -146,13 +155,27 @@ protected:
    */
   RCLCPP_COMPONENTS_PUBLIC
   virtual void
-  OnLoadNode(
+  on_load_node(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<LoadNode::Request> request,
     std::shared_ptr<LoadNode::Response> response);
 
+  /**
+   * \deprecated Use on_load_node() instead
+   */
+  [[deprecated("Use on_load_node() instead")]]
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual void
+  OnLoadNode(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<LoadNode::Request> request,
+    std::shared_ptr<LoadNode::Response> response)
+  {
+    on_load_node(request_header, request, response);
+  }
+
   /// Service callback to unload a node in the component
-  /*
+  /**
    * \param request_header unused
    * \param request unique identifier to remove from the component
    * \param response true on the success field if the node unload was succefully, otherwise false
@@ -160,13 +183,27 @@ protected:
    */
   RCLCPP_COMPONENTS_PUBLIC
   virtual void
-  OnUnloadNode(
+  on_unload_node(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<UnloadNode::Request> request,
     std::shared_ptr<UnloadNode::Response> response);
 
+  /**
+   * \deprecated Use on_unload_node() instead
+   */
+  [[deprecated("Use on_unload_node() instead")]]
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual void
+  OnUnloadNode(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<UnloadNode::Request> request,
+    std::shared_ptr<UnloadNode::Response> response)
+  {
+    on_unload_node(request_header, request, response);
+  }
+
   /// Service callback to get the list of nodes in the component
-  /*
+  /**
    * Return a two list: one with the unique identifiers and other with full name of the nodes.
    *
    * \param request_header unused
@@ -175,11 +212,25 @@ protected:
    */
   RCLCPP_COMPONENTS_PUBLIC
   virtual void
-  OnListNodes(
+  on_list_nodes(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<ListNodes::Request> request,
     std::shared_ptr<ListNodes::Response> response);
 
+  /**
+   * \deprecated Use on_list_nodes() instead
+   */
+  [[deprecated("Use on_list_nodes() instead")]]
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual void
+  OnListNodes(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<ListNodes::Request> request,
+    std::shared_ptr<ListNodes::Response> response)
+  {
+    on_list_nodes(request_header, request, response);
+  }
+
 private:
   std::weak_ptr<rclcpp::Executor> executor_;
 

rclcpp_components/package.xml

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

rclcpp_components/src/component_manager.cpp

@@ -39,13 +39,13 @@ ComponentManager::ComponentManager(
 {
   loadNode_srv_ = create_service<LoadNode>(
     "~/_container/load_node",
-    std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3));
   unloadNode_srv_ = create_service<UnloadNode>(
     "~/_container/unload_node",
-    std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3));
   listNodes_srv_ = create_service<ListNodes>(
     "~/_container/list_nodes",
-    std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3));
 }
 
 ComponentManager::~ComponentManager()
@@ -121,8 +121,53 @@ ComponentManager::create_component_factory(const ComponentResource & resource)
   return {};
 }
 
+rclcpp::NodeOptions
+ComponentManager::create_node_options(const std::shared_ptr<LoadNode::Request> request)
+{
+  std::vector<rclcpp::Parameter> parameters;
+  for (const auto & p : request->parameters) {
+    parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
+  }
+
+  std::vector<std::string> remap_rules;
+  remap_rules.reserve(request->remap_rules.size() * 2 + 1);
+  remap_rules.push_back("--ros-args");
+  for (const std::string & rule : request->remap_rules) {
+    remap_rules.push_back("-r");
+    remap_rules.push_back(rule);
+  }
+
+  if (!request->node_name.empty()) {
+    remap_rules.push_back("-r");
+    remap_rules.push_back("__node:=" + request->node_name);
+  }
+
+  if (!request->node_namespace.empty()) {
+    remap_rules.push_back("-r");
+    remap_rules.push_back("__ns:=" + request->node_namespace);
+  }
+
+  auto options = rclcpp::NodeOptions()
+    .use_global_arguments(false)
+    .parameter_overrides(parameters)
+    .arguments(remap_rules);
+
+  for (const auto & a : request->extra_arguments) {
+    const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a);
+    if (extra_argument.get_name() == "use_intra_process_comms") {
+      if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
+        throw ComponentManagerException(
+                "Extra component argument 'use_intra_process_comms' must be a boolean");
+      }
+      options.use_intra_process_comms(extra_argument.get_value<bool>());
+    }
+  }
+
+  return options;
+}
+
 void
-ComponentManager::OnLoadNode(
+ComponentManager::on_load_node(
   const std::shared_ptr<rmw_request_id_t> request_header,
   const std::shared_ptr<LoadNode::Request> request,
   std::shared_ptr<LoadNode::Response> response)
@@ -142,45 +187,7 @@ ComponentManager::OnLoadNode(
         continue;
       }
 
-      std::vector<rclcpp::Parameter> parameters;
-      for (const auto & p : request->parameters) {
-        parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
-      }
-
-      std::vector<std::string> remap_rules;
-      remap_rules.reserve(request->remap_rules.size() * 2 + 1);
-      remap_rules.push_back("--ros-args");
-      for (const std::string & rule : request->remap_rules) {
-        remap_rules.push_back("-r");
-        remap_rules.push_back(rule);
-      }
-
-      if (!request->node_name.empty()) {
-        remap_rules.push_back("-r");
-        remap_rules.push_back("__node:=" + request->node_name);
-      }
-
-      if (!request->node_namespace.empty()) {
-        remap_rules.push_back("-r");
-        remap_rules.push_back("__ns:=" + request->node_namespace);
-      }
-
-      auto options = rclcpp::NodeOptions()
-        .use_global_arguments(false)
-        .parameter_overrides(parameters)
-        .arguments(remap_rules);
-
-      for (const auto & a : request->extra_arguments) {
-        const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a);
-        if (extra_argument.get_name() == "use_intra_process_comms") {
-          if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
-            throw ComponentManagerException(
-                    "Extra component argument 'use_intra_process_comms' must be a boolean");
-          }
-          options.use_intra_process_comms(extra_argument.get_value<bool>());
-        }
-      }
-
+      auto options = create_node_options(request);
       auto node_id = unique_id_++;
 
       if (0 == node_id) {
@@ -230,7 +237,7 @@ ComponentManager::OnLoadNode(
 }
 
 void
-ComponentManager::OnUnloadNode(
+ComponentManager::on_unload_node(
   const std::shared_ptr<rmw_request_id_t> request_header,
   const std::shared_ptr<UnloadNode::Request> request,
   std::shared_ptr<UnloadNode::Response> response)
@@ -255,7 +262,7 @@ ComponentManager::OnUnloadNode(
 }
 
 void
-ComponentManager::OnListNodes(
+ComponentManager::on_list_nodes(
   const std::shared_ptr<rmw_request_id_t> request_header,
   const std::shared_ptr<ListNodes::Request> request,
   std::shared_ptr<ListNodes::Response> response)

rclcpp_components/test/test_component_manager_api.cpp

@@ -57,13 +57,14 @@ TEST_F(TestComponentManager, components_api)
     request->package_name = "rclcpp_components";
     request->plugin_name = "test_rclcpp_components::TestComponentFoo";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
-    EXPECT_EQ(result.get()->unique_id, 1u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    EXPECT_EQ(result->full_node_name, "/test_component_foo");
+    EXPECT_EQ(result->unique_id, 1u);
   }
 
   {
@@ -71,13 +72,14 @@ TEST_F(TestComponentManager, components_api)
     request->package_name = "rclcpp_components";
     request->plugin_name = "test_rclcpp_components::TestComponentBar";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    EXPECT_EQ(result.get()->full_node_name, "/test_component_bar");
-    EXPECT_EQ(result.get()->unique_id, 2u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    EXPECT_EQ(result->full_node_name, "/test_component_bar");
+    EXPECT_EQ(result->unique_id, 2u);
   }
 
   // Test remapping the node name
@@ -87,13 +89,14 @@ TEST_F(TestComponentManager, components_api)
     request->plugin_name = "test_rclcpp_components::TestComponentFoo";
     request->node_name = "test_component_baz";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    EXPECT_EQ(result.get()->full_node_name, "/test_component_baz");
-    EXPECT_EQ(result.get()->unique_id, 3u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    EXPECT_EQ(result->full_node_name, "/test_component_baz");
+    EXPECT_EQ(result->unique_id, 3u);
   }
 
   // Test remapping the node namespace
@@ -104,13 +107,14 @@ TEST_F(TestComponentManager, components_api)
     request->node_namespace = "/ns";
     request->node_name = "test_component_bing";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing");
-    EXPECT_EQ(result.get()->unique_id, 4u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    EXPECT_EQ(result->full_node_name, "/ns/test_component_bing");
+    EXPECT_EQ(result->unique_id, 4u);
   }
 
   {
@@ -119,13 +123,14 @@ TEST_F(TestComponentManager, components_api)
     request->package_name = "rclcpp_components";
     request->plugin_name = "test_rclcpp_components::TestComponent";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, false);
-    EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name.");
-    EXPECT_EQ(result.get()->full_node_name, "");
-    EXPECT_EQ(result.get()->unique_id, 0u);
+    auto result = future.get();
+    EXPECT_EQ(result->success, false);
+    EXPECT_EQ(result->error_message, "Failed to find class with the requested plugin name.");
+    EXPECT_EQ(result->full_node_name, "");
+    EXPECT_EQ(result->unique_id, 0u);
   }
 
   {
@@ -134,13 +139,14 @@ TEST_F(TestComponentManager, components_api)
     request->package_name = "rclcpp_components_foo";
     request->plugin_name = "test_rclcpp_components::TestComponentFoo";
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, false);
-    EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index");
-    EXPECT_EQ(result.get()->full_node_name, "");
-    EXPECT_EQ(result.get()->unique_id, 0u);
+    auto result = future.get();
+    EXPECT_EQ(result->success, false);
+    EXPECT_EQ(result->error_message, "Could not find requested resource in ament index");
+    EXPECT_EQ(result->full_node_name, "");
+    EXPECT_EQ(result->unique_id, 0u);
   }
 
   {
@@ -151,13 +157,14 @@ TEST_F(TestComponentManager, components_api)
     request->node_name = "test_component_remap";
     request->remap_rules.push_back("alice:=bob");
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    EXPECT_EQ(result.get()->full_node_name, "/test_component_remap");
-    EXPECT_EQ(result.get()->unique_id, 5u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    EXPECT_EQ(result->full_node_name, "/test_component_remap");
+    EXPECT_EQ(result->unique_id, 5u);
   }
 
   {
@@ -170,14 +177,15 @@ TEST_F(TestComponentManager, components_api)
       rclcpp::ParameterValue(true));
     request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg());
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, true);
-    EXPECT_EQ(result.get()->error_message, "");
-    std::cout << result.get()->full_node_name << std::endl;
-    EXPECT_EQ(result.get()->full_node_name, "/test_component_intra_process");
-    EXPECT_EQ(result.get()->unique_id, 6u);
+    EXPECT_EQ(result->success, true);
+    EXPECT_EQ(result->error_message, "");
+    std::cout << result->full_node_name << std::endl;
+    EXPECT_EQ(result->full_node_name, "/test_component_intra_process");
+    EXPECT_EQ(result->unique_id, 6u);
   }
 
   {
@@ -191,15 +199,16 @@ TEST_F(TestComponentManager, components_api)
       rclcpp::ParameterValue("hello"));
     request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg());
 
-    auto result = composition_client->async_send_request(request);
-    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    auto future = composition_client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+    auto result = future.get();
     EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-    EXPECT_EQ(result.get()->success, false);
+    EXPECT_EQ(result->success, false);
     EXPECT_EQ(
-      result.get()->error_message,
+      result->error_message,
       "Extra component argument 'use_intra_process_comms' must be a boolean");
-    EXPECT_EQ(result.get()->full_node_name, "");
-    EXPECT_EQ(result.get()->unique_id, 0u);
+    EXPECT_EQ(result->full_node_name, "");
+    EXPECT_EQ(result->unique_id, 0u);
   }
 
   auto node_names = node->get_node_names();
@@ -223,11 +232,12 @@ TEST_F(TestComponentManager, components_api)
 
     {
       auto request = std::make_shared<composition_interfaces::srv::ListNodes::Request>();
-      auto result = client->async_send_request(request);
-      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      auto future = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
       EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-      auto result_node_names = result.get()->full_node_names;
-      auto result_unique_ids = result.get()->unique_ids;
+      auto result = future.get();
+      auto result_node_names = result->full_node_names;
+      auto result_unique_ids = result->unique_ids;
 
       EXPECT_EQ(result_node_names.size(), 6u);
       EXPECT_EQ(result_node_names[0], "/test_component_foo");
@@ -258,22 +268,24 @@ TEST_F(TestComponentManager, components_api)
       auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
       request->unique_id = 1;
 
-      auto result = client->async_send_request(request);
-      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      auto future = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+      auto result = future.get();
       EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-      EXPECT_EQ(result.get()->success, true);
-      EXPECT_EQ(result.get()->error_message, "");
+      EXPECT_EQ(result->success, true);
+      EXPECT_EQ(result->error_message, "");
     }
 
     {
       auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
       request->unique_id = 1;
 
-      auto result = client->async_send_request(request);
-      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      auto future = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
+      auto result = future.get();
       EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-      EXPECT_EQ(result.get()->success, false);
-      EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1");
+      EXPECT_EQ(result->success, false);
+      EXPECT_EQ(result->error_message, "No node found with unique_id: 1");
     }
   }
 
@@ -287,11 +299,12 @@ TEST_F(TestComponentManager, components_api)
 
     {
       auto request = std::make_shared<composition_interfaces::srv::ListNodes::Request>();
-      auto result = client->async_send_request(request);
-      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      auto future = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(future, 5s);  // Wait for the result.
       EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS);
-      auto result_node_names = result.get()->full_node_names;
-      auto result_unique_ids = result.get()->unique_ids;
+      auto result = future.get();
+      auto result_node_names = result->full_node_names;
+      auto result_unique_ids = result->unique_ids;
 
       EXPECT_EQ(result_node_names.size(), 5u);
       EXPECT_EQ(result_node_names[0], "/test_component_bar");

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,42 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+13.1.0 (2021-10-18)
+-------------------
+* Update forward declarations of `rcl_lifecycle` types (`#1788 <https://github.com/ros2/rclcpp/issues/1788>`_)
+* Deprecate the `void shared_ptr<MessageT>` subscription callback signatures (`#1713 <https://github.com/ros2/rclcpp/issues/1713>`_)
+* Contributors: Abrar Rahman Protyasha, Michel Hidalgo
+
+13.0.0 (2021-08-23)
+-------------------
+* Update client API to be able to remove pending requests. (`#1734 <https://github.com/ros2/rclcpp/issues/1734>`_)
+* Change log level for lifecycle_publisher. (`#1715 <https://github.com/ros2/rclcpp/issues/1715>`_)
+* Fix: RCLCPP_PUBLIC -> RCLCPP_LIFECYCLE_PUBLIC (`#1732 <https://github.com/ros2/rclcpp/issues/1732>`_)
+* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp (`#1727 <https://github.com/ros2/rclcpp/issues/1727>`_)
+* Contributors: Alberto Soragna, Christophe Bedard, Ivan Santiago Paunovic, Shane Loretz
+
+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().
+* Contributors: Chris Lalancette
+
+11.2.0 (2021-07-21)
+-------------------
+
+11.1.0 (2021-07-13)
+-------------------
+
 11.0.0 (2021-05-18)
 -------------------
 * Fix destruction order in lifecycle benchmark (`#1675 <https://github.com/ros2/rclcpp/issues/1675>`_)

rclcpp_lifecycle/CMakeLists.txt

@@ -103,6 +103,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_lifecycle_service_client
       "rcl_lifecycle"
       "rclcpp"
+      "rcpputils"
       "rcutils"
     )
     target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick)

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -192,13 +192,11 @@ public:
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
-  /// Return the list of callback groups in the node.
-  /**
-   * \return list of callback groups in the node.
-   */
+  /// Iterate over the callback groups in the node, calling func on each valid one.
   RCLCPP_LIFECYCLE_PUBLIC
-  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-  get_callback_groups() const;
+  void
+  for_each_callback_group(
+    const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
 
   /// Create and return a Publisher.
   /**

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp

@@ -65,6 +65,7 @@ public:
     const rclcpp::PublisherOptionsWithAllocator<Alloc> & options)
   : rclcpp::Publisher<MessageT, Alloc>(node_base, topic, qos, options),
     enabled_(false),
+    should_log_(true),
     logger_(rclcpp::get_logger("LifecyclePublisher"))
   {
   }
@@ -81,11 +82,7 @@ public:
   publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     if (!enabled_) {
-      RCLCPP_WARN(
-        logger_,
-        "Trying to publish message on the topic '%s', but the publisher is not activated",
-        this->get_topic_name());
-
+      log_publisher_not_enabled();
       return;
     }
     rclcpp::Publisher<MessageT, Alloc>::publish(std::move(msg));
@@ -101,11 +98,7 @@ public:
   publish(const MessageT & msg)
   {
     if (!enabled_) {
-      RCLCPP_WARN(
-        logger_,
-        "Trying to publish message on the topic '%s', but the publisher is not activated",
-        this->get_topic_name());
-
+      log_publisher_not_enabled();
       return;
     }
     rclcpp::Publisher<MessageT, Alloc>::publish(msg);
@@ -121,6 +114,7 @@ public:
   on_deactivate()
   {
     enabled_ = false;
+    should_log_ = true;
   }
 
   virtual bool
@@ -130,7 +124,30 @@ public:
   }
 
 private:
+  /// LifecyclePublisher log helper function
+  /**
+   * Helper function that logs a message saying that publisher can't publish
+   * because it's not enabled.
+   */
+  void log_publisher_not_enabled()
+  {
+    // Nothing to do if we are not meant to log
+    if (!should_log_) {
+      return;
+    }
+
+    // Log the message
+    RCLCPP_WARN(
+      logger_,
+      "Trying to publish message on the topic '%s', but the publisher is not activated",
+      this->get_topic_name());
+
+    // We stop logging until the flag gets enabled again
+    should_log_ = false;
+  }
+
   bool enabled_ = false;
+  bool should_log_ = true;
   rclcpp::Logger logger_;
 };
 

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -17,13 +17,11 @@
 
 #include <string>
 
+#include "rcl_lifecycle/data_types.h"
 #include "rclcpp_lifecycle/visibility_control.h"
 
 #include "rcutils/allocator.h"
 
-// forward declare rcl_state_t
-typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t;
-
 namespace rclcpp_lifecycle
 {
 

rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp

@@ -17,14 +17,12 @@
 
 #include <string>
 
+#include "rcl_lifecycle/data_types.h"
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
 
 #include "rcutils/allocator.h"
 
-// forward declare rcl_transition_t
-typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t;
-
 namespace rclcpp_lifecycle
 {
 

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>11.0.0</version>
+  <version>13.1.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
@@ -29,6 +29,7 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>mimick_vendor</test_depend>
   <test_depend>performance_test_fixture</test_depend>
+  <test_depend>rcpputils</test_depend>
   <test_depend>rcutils</test_depend>
   <test_depend>test_msgs</test_depend>
 

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -365,10 +365,11 @@ LifecycleNode::get_subscriptions_info_by_topic(const std::string & topic_name, b
   return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle);
 }
 
-const std::vector<rclcpp::CallbackGroup::WeakPtr> &
-LifecycleNode::get_callback_groups() const
+void
+LifecycleNode::for_each_callback_group(
+  const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
 {
-  return node_base_->get_callback_groups();
+  node_base_->for_each_callback_group(func);
 }
 
 rclcpp::Event::SharedPtr

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -416,7 +416,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) {
 TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
   auto test_node = std::make_shared<MoodyLifecycleNode<GoodMood>>("testnode");
 
-  auto cb = [](const std::shared_ptr<lifecycle_msgs::msg::State> msg) {(void) msg;};
+  auto cb = [](const std::shared_ptr<const lifecycle_msgs::msg::State> msg) {(void) msg;};
   auto lifecycle_sub =
     test_node->create_subscription<lifecycle_msgs::msg::State>("~/empty", 10, cb);
 
@@ -717,16 +717,27 @@ TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {
 
 TEST_F(TestDefaultStateMachine, test_callback_groups) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
-  auto groups = test_node->get_callback_groups();
-  EXPECT_EQ(groups.size(), 1u);
+  size_t num_groups = 0;
+  test_node->for_each_callback_group(
+    [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      (void)group_ptr;
+      num_groups++;
+    });
+  EXPECT_EQ(num_groups, 1u);
 
   auto group = test_node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive, true);
   EXPECT_NE(nullptr, group);
 
-  groups = test_node->get_callback_groups();
-  EXPECT_EQ(groups.size(), 2u);
-  EXPECT_EQ(groups[1].lock().get(), group.get());
+  num_groups = 0;
+  test_node->for_each_callback_group(
+    [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      (void)group_ptr;
+      num_groups++;
+    });
+  EXPECT_EQ(num_groups, 2u);
 }
 
 TEST_F(TestDefaultStateMachine, wait_for_graph_change)