13.0.0

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,59 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+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/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/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/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/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/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/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<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.0.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;

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/node.cpp

@@ -61,6 +61,12 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
             extension.c_str(),
             "a sub-namespace should not have a leading /",
             0);
+  } else if (existing_sub_namespace.empty() && extension.empty()) {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "sub-nodes should not extend nodes by an empty sub-namespace",
+            0);
   }
 
   std::string new_sub_namespace;
@@ -86,7 +92,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 {
@@ -482,10 +492,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_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/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/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/utilities.cpp

@@ -156,12 +156,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_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_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_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_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,22 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+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.0.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>
@@ -1082,7 +1082,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,24 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+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.0.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,36 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+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/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.0.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

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

rclcpp_lifecycle/test/test_lifecycle_service_client.cpp

@@ -37,6 +37,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "./mocking_utils/patch.hpp"
 
 using namespace std::chrono_literals;
@@ -96,8 +98,9 @@ public:
       return unknown_state;
     }
 
-    if (future_result.get()) {
-      return future_result.get()->current_state;
+    auto result = future_result.get();
+    if (result) {
+      return result->current_state;
     } else {
       return unknown_state;
     }
@@ -138,9 +141,9 @@ public:
     if (future_status != std::future_status::ready) {
       return std::vector<lifecycle_msgs::msg::State>();
     }
-
-    if (future_result.get()) {
-      return future_result.get()->available_states;
+    auto result = future_result.get();
+    if (result) {
+      return result->available_states;
     }
 
     return std::vector<lifecycle_msgs::msg::State>();
@@ -162,8 +165,9 @@ public:
       return std::vector<lifecycle_msgs::msg::TransitionDescription>();
     }
 
-    if (future_result.get()) {
-      return future_result.get()->available_transitions;
+    auto result = future_result.get();
+    if (result) {
+      return result->available_transitions;
     }
 
     return std::vector<lifecycle_msgs::msg::TransitionDescription>();
@@ -185,8 +189,9 @@ public:
       return std::vector<lifecycle_msgs::msg::TransitionDescription>();
     }
 
-    if (future_result.get()) {
-      return future_result.get()->available_transitions;
+    auto result = future_result.get();
+    if (result) {
+      return result->available_transitions;
     }
 
     return std::vector<lifecycle_msgs::msg::TransitionDescription>();
@@ -395,7 +400,8 @@ TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values)
     };
 
   auto handler = node1->add_on_set_parameters_callback(on_set_parameters);
-  RCLCPP_SCOPE_EXIT({node1->remove_on_set_parameters_callback(handler.get());});    // always reset
+  RCPPUTILS_SCOPE_EXIT(
+    {node1->remove_on_set_parameters_callback(handler.get());});  // always reset
 }
 
 TEST_F(TestLifecycleServiceClient, wait_for_graph_change)