9.2.2

(cherry picked from commit 3cddb4edab)

Signed-off-by: Markus Hofstaetter <markus.hofstaetter@ait.ac.at>
Co-authored-by: M. Hofstätter <markus.hofstaetter@gmx.net>

README.md

@@ -8,10 +8,10 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+Visit the [rclcpp API documentation](http://docs.ros2.org/galactic/api/rclcpp/) for a complete list of its main components.
 
 ### Examples
 
-The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
-and [Writing a simple service and client](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/galactic/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
+and [Writing a simple service and client](https://docs.ros.org/en/galactic/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
 contain some examples of rclcpp APIs in use.

rclcpp/CHANGELOG.rst

@@ -2,90 +2,39 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-13.1.0 (2021-10-18)
--------------------
-* Fix dangerous std::bind capture in TimeSource implementation. (`#1768 <https://github.com/ros2/rclcpp/issues/1768>`_)
-* Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 <https://github.com/ros2/rclcpp/issues/1770>`_)
-* Handle sigterm, in the same way sigint is being handled. (`#1771 <https://github.com/ros2/rclcpp/issues/1771>`_)
-* rclcpp::Node copy constructor: make copy of node_waitables\_ member. (`#1799 <https://github.com/ros2/rclcpp/issues/1799>`_)
-* Extend NodeGraph to match what rcl provides. (`#1484 <https://github.com/ros2/rclcpp/issues/1484>`_)
-* Context::sleep_for(): replace recursion with do-while to avoid potential stack-overflow. (`#1765 <https://github.com/ros2/rclcpp/issues/1765>`_)
-* extend_sub_namespace(): Verify string::empty() before calling string::front(). (`#1764 <https://github.com/ros2/rclcpp/issues/1764>`_)
-* Deprecate the `void shared_ptr<MessageT>` subscription callback signatures. (`#1713 <https://github.com/ros2/rclcpp/issues/1713>`_)
-* Contributors: Abrar Rahman Protyasha, Chris Lalancette, Emerson Knapp, Geoffrey Biggs, Ivan Santiago Paunovic, Jorge Perez, Tomoya Fujita, William Woodall, Yong-Hao Zou, livanov93
+9.2.2 (2022-12-06)
+------------------
+* Fix returning invalid namespace if sub_namespace is empty (`#1810 <https://github.com/ros2/rclcpp/issues/1810>`_)
+* use regex for wildcard matching (`#1987 <https://github.com/ros2/rclcpp/issues/1987>`_)
+* Add statistics for handle_loaned_message (`#1933 <https://github.com/ros2/rclcpp/issues/1933>`_)
+* Contributors: Aaron Lipinski, Barry Xu, Chen Lihui, M. Hofstätter
 
-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
+9.2.1 (2022-04-28)
+------------------
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_) (`#1909 <https://github.com/ros2/rclcpp/issues/1909>`_)
+* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_) (`#1822 <https://github.com/ros2/rclcpp/issues/1822>`_)
+* Contributors: mergify[bot]
 
-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
+9.2.0 (2021-09-17)
+------------------
+* Added thread safe for_each_callback_group method (`#1741 <https://github.com/ros2/rclcpp/issues/1741>`_)
+* Added a function rclcpp::wait_for_message() convenience function (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_) (`#1740 <https://github.com/ros2/rclcpp/issues/1740>`_)
+* Fixed a documentation bug (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_) (`#1720 <https://github.com/ros2/rclcpp/issues/1720>`_)
+* Contributors: Aditya Pande, Karsten Knese, mergify[bot]
 
-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
+9.1.0 (2021-05-20)
+------------------
+* Allow declaring uninitialized statically typed parameters. (`#1673 <https://github.com/ros2/rclcpp/issues/1673>`_) (`#1681 <https://github.com/ros2/rclcpp/issues/1681>`_)
+* [service] Don't use a weak_ptr to avoid leaking. (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_) (`#1670 <https://github.com/ros2/rclcpp/issues/1670>`_)
+* Contributors: Jacob Perron, Ivan Santiago Paunovic
 
-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>`_)
-* Fix syntax issue with gcc (`#1674 <https://github.com/ros2/rclcpp/issues/1674>`_)
-* [service] Don't use a weak_ptr to avoid leaking (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_)
-* Contributors: Ivan Santiago Paunovic, Jacob Perron, William Woodall
-
-10.0.0 (2021-05-11)
--------------------
-* Fix doc typo (`#1663 <https://github.com/ros2/rclcpp/issues/1663>`_)
-* [rclcpp] Type Adaptation feature (`#1557 <https://github.com/ros2/rclcpp/issues/1557>`_)
-* Do not attempt to use void allocators for memory allocation. (`#1657 <https://github.com/ros2/rclcpp/issues/1657>`_)
-* Keep custom allocator in publisher and subscription options alive. (`#1647 <https://github.com/ros2/rclcpp/issues/1647>`_)
-* Fix get_publishers_subscriptions_info_by_topic test in test_node.cpp (`#1648 <https://github.com/ros2/rclcpp/issues/1648>`_)
-* Use OnShutdown callback handle instead of OnShutdown callback (`#1639 <https://github.com/ros2/rclcpp/issues/1639>`_)
-* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (`#1643 <https://github.com/ros2/rclcpp/issues/1643>`_)
+9.0.3 (2021-05-10)
+------------------
+* Use OnShutdown callback handle instead of OnShutdown callback (`#1639 <https://github.com/ros2/rclcpp/issues/1639>`_) (`#1650 <https://github.com/ros2/rclcpp/issues/1650>`_)
+* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (`#1643 <https://github.com/ros2/rclcpp/issues/1643>`_) (`#1644 <https://github.com/ros2/rclcpp/issues/1644>`_)
 * Increase cppcheck timeout to 500s (`#1634 <https://github.com/ros2/rclcpp/issues/1634>`_)
 * Clarify node parameters docs (`#1631 <https://github.com/ros2/rclcpp/issues/1631>`_)
-* Contributors: Audrow Nash, Barry Xu, Jacob Perron, Michel Hidalgo, Shane Loretz, William Woodall
+* Contributors: Barry Xu, Jacob Perron, Michel Hidalgo, Shane Loretz, William Woodall
 
 9.0.2 (2021-04-14)
 ------------------

rclcpp/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.12)
+cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
@@ -41,6 +41,7 @@ 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
@@ -109,8 +110,6 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/waitable.cpp
 )
 
-find_package(Python3 REQUIRED COMPONENTS Interpreter)
-
 # "watch" template for changes
 configure_file(
   "resource/logging.hpp.em"
@@ -124,7 +123,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 Python3::Interpreter ARGS -c "${python_code_logging}"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
   DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
   COMMENT "Expanding logging.hpp.em"
   VERBATIM
@@ -148,7 +147,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 Python3::Interpreter ARGS -c "${python_${interface_name}_traits}"
+    COMMAND ${PYTHON_EXECUTABLE} 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
@@ -168,7 +167,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 Python3::Interpreter ARGS -c "${python_get_${interface_name}}"
+    COMMAND ${PYTHON_EXECUTABLE} 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
@@ -253,5 +252,3 @@ if(TEST cppcheck)
   # must set the property after ament_package()
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
-
-ament_cmake_gen_version_h()

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -95,7 +95,7 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
   std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
-  std::allocator<T>>::value,
+  typename std::allocator<void>::template rebind<T>::other>::value,
   std::default_delete<T>,
   AllocatorDeleter<Alloc>
   >::type;

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -15,12 +15,10 @@
 #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"
@@ -31,204 +29,93 @@
 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()
-  : callback_(std::monostate{})
+  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
   {}
 
+  AnyServiceCallback(const AnyServiceCallback &) = default;
+
   template<
     typename CallbackT,
-    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 (
+    typename std::enable_if<
       rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
-      >::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);
-    }
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_callback_ = callback;
   }
 
   template<
     typename CallbackT,
-    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
+    typename std::enable_if<
       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);
-    }
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_with_request_header_callback_ = callback;
   }
 
-  // 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)
+  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)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), false);
-    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_)) {
+    if (shared_ptr_callback_ != nullptr) {
       (void)request_header;
-      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);
+      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");
     }
     TRACEPOINT(callback_end, static_cast<const void *>(this));
-    return response;
   }
 
   void register_callback_for_tracing()
   {
 #ifndef TRACETOOLS_DISABLED
-    std::visit(
-      [this](auto && arg) {
-        TRACEPOINT(
-          rclcpp_callback_register,
-          static_cast<const void *>(this),
-          tracetools::get_symbol(arg));
-      }, callback_);
+    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_));
+    }
 #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,15 +17,12 @@
 
 #include <atomic>
 #include <future>
-#include <unordered_map>
+#include <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"
@@ -48,68 +45,6 @@
 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;
@@ -243,9 +178,6 @@ 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;
 
@@ -255,7 +187,6 @@ 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>>;
 
@@ -264,64 +195,6 @@ 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.
@@ -419,89 +292,34 @@ public:
     std::shared_ptr<rmw_request_id_t> request_header,
     std::shared_ptr<void> response) override
   {
-    std::optional<CallbackInfoVariant>
-    optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number);
-    if (!optional_pending_request) {
+    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...");
       return;
     }
-    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));
-    }
+    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);
   }
 
-  /// 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
+  SharedFuture
   async_send_request(SharedRequest request)
   {
-    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);
+    return async_send_request(request, [](SharedFuture) {});
   }
 
-  /// 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<
@@ -511,28 +329,23 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureAndRequestId
+  SharedFuture
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    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};
+    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;
   }
 
-  /// 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<
@@ -542,165 +355,28 @@ public:
       >::value
     >::type * = nullptr
   >
-  SharedFutureWithRequestAndRequestId
+  SharedFutureWithRequest
   async_send_request(SharedRequest request, CallbackT && cb)
   {
-    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;
+    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;
   }
 
+private:
   RCLCPP_DISABLE_COPY(Client)
 
-  std::unordered_map<
-    int64_t,
-    std::pair<
-      std::chrono::time_point<std::chrono::system_clock>,
-      CallbackInfoVariant>>
-  pending_requests_;
+  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
   std::mutex pending_requests_mutex_;
 };
 

rclcpp/include/rclcpp/clock.hpp

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

rclcpp/include/rclcpp/context.hpp

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

rclcpp/include/rclcpp/create_subscription.hpp

@@ -47,11 +47,11 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT,
+  typename CallbackMessageT,
   typename SubscriptionT,
   typename MessageMemoryStrategyT,
   typename NodeParametersT,
-  typename NodeTopicsT,
-  typename ROSMessageType = typename SubscriptionT::ROSMessageType>
+  typename NodeTopicsT>
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
   NodeParametersT & node_parameters,
@@ -70,7 +70,7 @@ create_subscription(
   using rclcpp::node_interfaces::get_node_topics_interface;
   auto node_topics_interface = get_node_topics_interface(node_topics);
 
-  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
   subscription_topic_stats = nullptr;
 
   if (rclcpp::detail::resolve_enable_topic_statistics(
@@ -92,11 +92,11 @@ create_subscription(
       qos);
 
     subscription_topic_stats = std::make_shared<
-      rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
       >(node_topics_interface->get_node_base_interface()->get_name(), publisher);
 
     std::weak_ptr<
-      rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
     > weak_subscription_topic_stats(subscription_topic_stats);
     auto sub_call_back = [weak_subscription_topic_stats]() {
         auto subscription_topic_stats = weak_subscription_topic_stats.lock();
@@ -153,6 +153,7 @@ create_subscription(
  * \tparam MessageT
  * \tparam CallbackT
  * \tparam AllocatorT
+ * \tparam CallbackMessageT
  * \tparam SubscriptionT
  * \tparam MessageMemoryStrategyT
  * \tparam NodeT
@@ -170,8 +171,13 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT = std::allocator<void>,
-  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
-  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >,
   typename NodeT>
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
@@ -188,7 +194,7 @@ create_subscription(
 )
 {
   return rclcpp::detail::create_subscription<
-    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>(
     node, node, topic_name, qos, std::forward<CallbackT>(callback), options, msg_mem_strat);
 }
 
@@ -200,8 +206,13 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT = std::allocator<void>,
-  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
-  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >>
 typename std::shared_ptr<SubscriptionT>
 create_subscription(
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
@@ -218,7 +229,7 @@ create_subscription(
 )
 {
   return rclcpp::detail::create_subscription<
-    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>(
     node_parameters, node_topics, topic_name, qos,
     std::forward<CallbackT>(callback), options, msg_mem_strat);
 }

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

@@ -0,0 +1,76 @@
+// 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,7 +29,6 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
-#include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -41,6 +40,7 @@
 #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");
     }
-    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+    RCLCPP_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,6 +22,7 @@
 #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"
@@ -81,10 +82,12 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;
+  detail::MutexTwoPriorities 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,13 +57,15 @@ 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);
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
+    rcl_guard_condition_t * executor_guard_condition);
 
   /// Finalize StaticExecutorEntitiesCollector to clear resources
   RCLCPP_PUBLIC
@@ -336,10 +338,6 @@ private:
   /// List of weak nodes registered in the static executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 
-  // Mutex to protect vector of new nodes.
-  std::mutex new_nodes_mutex_;
-  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
-
   /// Wait set for managing entities that the rmw layer waits on.
   rcl_wait_set_t * p_wait_set_ = nullptr;
 

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

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

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

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

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -16,13 +16,14 @@
 #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
-#include <stdexcept>
+#include <type_traits>
 #include <utility>
 
+#include "rcl/subscription.h"
+
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
 #include "rclcpp/intra_process_buffer_type.hpp"
-#include "rclcpp/qos.hpp"
 
 namespace rclcpp
 {
@@ -36,13 +37,13 @@ template<
 typename rclcpp::experimental::buffers::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
 create_intra_process_buffer(
   IntraProcessBufferType buffer_type,
-  const rclcpp::QoS & qos,
+  rmw_qos_profile_t qos,
   std::shared_ptr<Alloc> allocator)
 {
   using MessageSharedPtr = std::shared_ptr<const MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
 
-  size_t buffer_size = qos.depth();
+  size_t buffer_size = qos.depth;
 
   using rclcpp::experimental::buffers::IntraProcessBuffer;
   typename IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;

rclcpp/include/rclcpp/experimental/executable_list.hpp

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

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

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -15,22 +15,23 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 
-#include <rmw/types.h>
+#include <rmw/rmw.h>
 
+#include <functional>
+#include <map>
 #include <memory>
 #include <stdexcept>
 #include <string>
-#include <type_traits>
 #include <utility>
 
-#include "rcl/types.h"
+#include "rcl/error_handling.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
-#include "rclcpp/context.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
-#include "rclcpp/qos.hpp"
+#include "rclcpp/experimental/create_intra_process_buffer.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -43,43 +44,54 @@ template<
   typename Alloc = std::allocator<void>,
   typename Deleter = std::default_delete<MessageT>,
   typename CallbackMessageT = MessageT>
-class SubscriptionIntraProcess
-  : public SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
-  >
+class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 {
-  using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
-  >;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
 
-  using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits;
-  using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc;
-  using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr;
-  using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr;
-  using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
+    MessageT,
+    Alloc,
+    Deleter
+    >::UniquePtr;
 
   SubscriptionIntraProcess(
     AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
-    const rclcpp::QoS & qos_profile,
+    rmw_qos_profile_t qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(
-      allocator,
-      context,
-      topic_name,
-      qos_profile,
-      buffer_type),
+  : SubscriptionIntraProcessBase(topic_name, qos_profile),
     any_callback_(callback)
   {
+    if (!std::is_same<MessageT, CallbackMessageT>::value) {
+      throw std::runtime_error("SubscriptionIntraProcess wrong callback type");
+    }
+
+    // Create the intra-process buffer.
+    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
+      buffer_type,
+      qos_profile,
+      allocator);
+
+    // Create the guard condition.
+    rcl_guard_condition_options_t guard_condition_options =
+      rcl_guard_condition_get_default_options();
+
+    gc_ = rcl_get_zero_initialized_guard_condition();
+    rcl_ret_t ret = rcl_guard_condition_init(
+      &gc_, context->get_rcl_context().get(), guard_condition_options);
+
+    if (RCL_RET_OK != ret) {
+      throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition");
+    }
+
     TRACEPOINT(
       rclcpp_subscription_callback_added,
       static_cast<const void *>(this),
@@ -92,7 +104,22 @@ public:
 #endif
   }
 
-  virtual ~SubscriptionIntraProcess() = default;
+  ~SubscriptionIntraProcess()
+  {
+    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Failed to destroy guard condition: %s",
+        rcutils_get_error_string().str);
+    }
+  }
+
+  bool
+  is_ready(rcl_wait_set_t * wait_set)
+  {
+    (void) wait_set;
+    return buffer_->has_data();
+  }
 
   std::shared_ptr<void>
   take_data()
@@ -101,9 +128,9 @@ public:
     MessageUniquePtr unique_msg;
 
     if (any_callback_.use_take_shared_method()) {
-      shared_msg = this->buffer_->consume_shared();
+      shared_msg = buffer_->consume_shared();
     } else {
-      unique_msg = this->buffer_->consume_unique();
+      unique_msg = buffer_->consume_unique();
     }
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
@@ -114,10 +141,37 @@ public:
 
   void execute(std::shared_ptr<void> & data)
   {
-    execute_impl<MessageT>(data);
+    execute_impl<CallbackMessageT>(data);
+  }
+
+  void
+  provide_intra_process_message(ConstMessageSharedPtr message)
+  {
+    buffer_->add_shared(std::move(message));
+    trigger_guard_condition();
+  }
+
+  void
+  provide_intra_process_message(MessageUniquePtr message)
+  {
+    buffer_->add_unique(std::move(message));
+    trigger_guard_condition();
+  }
+
+  bool
+  use_take_shared_method() const
+  {
+    return buffer_->use_take_shared_method();
+  }
+
+private:
+  void
+  trigger_guard_condition()
+  {
+    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
+    (void)ret;
   }
 
-protected:
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
   execute_impl(std::shared_ptr<void> & data)
@@ -143,15 +197,16 @@ protected:
 
     if (any_callback_.use_take_shared_method()) {
       ConstMessageSharedPtr shared_msg = shared_ptr->first;
-      any_callback_.template dispatch_intra_process<T>(shared_msg, msg_info);
+      any_callback_.dispatch_intra_process(shared_msg, msg_info);
     } else {
       MessageUniquePtr unique_msg = std::move(shared_ptr->second);
-      any_callback_.template dispatch_intra_process<T>(std::move(unique_msg), msg_info);
+      any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info);
     }
     shared_ptr.reset();
   }
 
   AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  BufferUniquePtr buffer_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -15,14 +15,16 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 
+#include <rmw/rmw.h>
+
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <utility>
 
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
+#include "rcl/error_handling.h"
 
-#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -37,9 +39,7 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
   RCLCPP_PUBLIC
-  SubscriptionIntraProcessBase(
-    const std::string & topic_name,
-    const rclcpp::QoS & qos_profile)
+  SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
   : topic_name_(topic_name), qos_profile_(qos_profile)
   {}
 
@@ -71,7 +71,7 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  QoS
+  rmw_qos_profile_t
   get_actual_qos() const;
 
 protected:
@@ -83,7 +83,7 @@ private:
   trigger_guard_condition() = 0;
 
   std::string topic_name_;
-  QoS qos_profile_;
+  rmw_qos_profile_t qos_profile_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -1,142 +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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
-#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
-
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <utility>
-
-#include "rcl/error_handling.h"
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
-#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
-#include "rclcpp/experimental/create_intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_base.hpp"
-#include "rclcpp/qos.hpp"
-#include "rclcpp/type_support_decl.hpp"
-
-namespace rclcpp
-{
-namespace experimental
-{
-
-template<
-  typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>
->
-class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer)
-
-  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
-  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
-
-  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
-    >::UniquePtr;
-
-  SubscriptionIntraProcessBuffer(
-    std::shared_ptr<Alloc> allocator,
-    rclcpp::Context::SharedPtr context,
-    const std::string & topic_name,
-    const rclcpp::QoS & qos_profile,
-    rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(topic_name, qos_profile)
-  {
-    // Create the intra-process buffer.
-    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
-      buffer_type,
-      qos_profile,
-      allocator);
-
-    // Create the guard condition.
-    rcl_guard_condition_options_t guard_condition_options =
-      rcl_guard_condition_get_default_options();
-
-    gc_ = rcl_get_zero_initialized_guard_condition();
-    rcl_ret_t ret = rcl_guard_condition_init(
-      &gc_, context->get_rcl_context().get(), guard_condition_options);
-
-    if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              "SubscriptionIntraProcessBuffer init error initializing guard condition");
-    }
-  }
-
-  virtual ~SubscriptionIntraProcessBuffer()
-  {
-    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Failed to destroy guard condition: %s",
-        rcutils_get_error_string().str);
-    }
-  }
-
-  bool
-  is_ready(rcl_wait_set_t * wait_set)
-  {
-    (void) wait_set;
-    return buffer_->has_data();
-  }
-
-  void
-  provide_intra_process_message(ConstMessageSharedPtr message)
-  {
-    buffer_->add_shared(std::move(message));
-    trigger_guard_condition();
-  }
-
-  void
-  provide_intra_process_message(MessageUniquePtr message)
-  {
-    buffer_->add_unique(std::move(message));
-    trigger_guard_condition();
-  }
-
-  bool
-  use_take_shared_method() const
-  {
-    return buffer_->use_take_shared_method();
-  }
-
-protected:
-  void
-  trigger_guard_condition()
-  {
-    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
-    if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret,
-        "Failed to trigger guard condition in intra-process buffer");
-    }
-  }
-
-  BufferUniquePtr buffer_;
-};
-
-}  // namespace experimental
-}  // namespace rclcpp
-
-#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -136,13 +136,6 @@ public:
   void handle_message(
     std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
 
-  /// Handle dispatching rclcpp::SerializedMessage to user callback.
-  RCLCPP_PUBLIC
-  void
-  handle_serialized_message(
-    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
-    const rclcpp::MessageInfo & message_info) override;
-
   /// This function is currently not implemented.
   RCLCPP_PUBLIC
   void handle_loaned_message(

rclcpp/include/rclcpp/get_message_type_support_handle.hpp

@@ -1,90 +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__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_
-#define RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_
-
-#include <type_traits>
-
-#include "rosidl_runtime_cpp/traits.hpp"
-#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
-#include "rosidl_typesupport_cpp/message_type_support.hpp"
-
-#include "rclcpp/type_adapter.hpp"
-
-/// Versions of rosidl_typesupport_cpp::get_message_type_support_handle that handle adapted types.
-
-namespace rclcpp
-{
-
-/// Specialization for when MessageT is an actual ROS message type.
-template<typename MessageT>
-constexpr
-typename std::enable_if_t<
-  rosidl_generator_traits::is_message<MessageT>::value,
-  const rosidl_message_type_support_t &
->
-get_message_type_support_handle()
-{
-  auto handle = rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
-  if (!handle) {
-    throw std::runtime_error("Type support handle unexpectedly nullptr");
-  }
-  return *handle;
-}
-
-/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter.
-template<typename AdaptedType>
-constexpr
-typename std::enable_if_t<
-  !rosidl_generator_traits::is_message<AdaptedType>::value &&
-  rclcpp::TypeAdapter<AdaptedType>::is_specialized::value,
-  const rosidl_message_type_support_t &
->
-get_message_type_support_handle()
-{
-  auto handle = rosidl_typesupport_cpp::get_message_type_support_handle<
-    typename TypeAdapter<AdaptedType>::ros_message_type
-    >();
-  if (!handle) {
-    throw std::runtime_error("Type support handle unexpectedly nullptr");
-  }
-  return *handle;
-}
-
-/// Specialization for when MessageT is not a ROS message nor an adapted type.
-/**
- * This specialization is a pass through runtime check, which allows a better
- * static_assert to catch this issue further down the line.
- * This should never get to be called in practice, and is purely defensive.
- */
-template<
-  typename AdaptedType
->
-constexpr
-typename std::enable_if_t<
-  !rosidl_generator_traits::is_message<AdaptedType>::value &&
-  !TypeAdapter<AdaptedType>::is_specialized::value,
-  const rosidl_message_type_support_t &
->
-get_message_type_support_handle()
-{
-  throw std::runtime_error(
-          "this specialization of rclcpp::get_message_type_support_handle() "
-          "should never be called");
-}
-
-}  // namespace rclcpp
-
-#endif  // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_

rclcpp/include/rclcpp/init_options.hpp

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

rclcpp/include/rclcpp/is_ros_compatible_type.hpp

@@ -1,35 +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__IS_ROS_COMPATIBLE_TYPE_HPP_
-#define RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_
-
-#include "rosidl_runtime_cpp/traits.hpp"
-
-#include "rclcpp/type_adapter.hpp"
-
-namespace rclcpp
-{
-
-template<typename T>
-struct is_ros_compatible_type
-{
-  static constexpr bool value =
-    rosidl_generator_traits::is_message<T>::value ||
-    rclcpp::TypeAdapter<T>::is_specialized::value;
-};
-
-}  // namespace rclcpp
-
-#endif  // RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_

rclcpp/include/rclcpp/loaned_message.hpp

@@ -103,9 +103,6 @@ public:
    * \param[in] allocator Allocator instance in case middleware can not allocate messages
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
-  [[
-    deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator")
-  ]]
   LoanedMessage(
     const rclcpp::PublisherBase * pub,
     std::shared_ptr<std::allocator<MessageT>> allocator)
@@ -117,9 +114,7 @@ public:
   : pub_(std::move(other.pub_)),
     message_(std::move(other.message_)),
     message_allocator_(std::move(other.message_allocator_))
-  {
-    other.message_ = nullptr;
-  }
+  {}
 
   /// Destructor of the LoanedMessage class.
   /**

rclcpp/include/rclcpp/node.hpp

@@ -148,6 +148,11 @@ public:
     rclcpp::CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
+  /// Return the list of callback groups in the node.
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const;
+
   /// 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
@@ -212,8 +217,13 @@ public:
     typename MessageT,
     typename CallbackT,
     typename AllocatorT = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
-    typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType
+    typename CallbackMessageT =
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+    typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+    typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      CallbackMessageT,
+      AllocatorT
+    >
   >
   std::shared_ptr<SubscriptionT>
   create_subscription(

rclcpp/include/rclcpp/node_impl.hpp

@@ -86,6 +86,7 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT,
+  typename CallbackMessageT,
   typename SubscriptionT,
   typename MessageMemoryStrategyT>
 std::shared_ptr<SubscriptionT>

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,10 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
-#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rcl/node.h"
@@ -33,12 +33,34 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+RCLCPP_PUBLIC
+void global_for_each_callback_group(
+  NodeBaseInterface * node_base_interface,
+  const NodeBaseInterface::CallbackGroupFunction & func);
+
+// Class to hold the global map of mutexes
+class map_of_mutexes final
+{
+public:
+  // Methods need to be protected by internal mutex
+  void create_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+  std::shared_ptr<std::mutex>
+  get_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+  void delete_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+
+private:
+  std::unordered_map<const NodeBaseInterface *, std::shared_ptr<std::mutex>> data_;
+  std::mutex internal_mutex_;
+};
+
 /// Implementation of the NodeBase part of the Node API.
 class NodeBase : public NodeBaseInterface
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
 
+  static map_of_mutexes map_object;
+
   RCLCPP_PUBLIC
   NodeBase(
     const std::string & node_name,
@@ -98,16 +120,9 @@ 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
-  void
-  for_each_callback_group(const CallbackGroupFunction & func) override;
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const override;
 
   RCLCPP_PUBLIC
   std::atomic_bool &
@@ -142,7 +157,6 @@ 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

@@ -38,6 +38,8 @@ class NodeBaseInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
+  using CallbackGroupFunction = std::function<void (rclcpp::CallbackGroup::SharedPtr)>;
+
   RCLCPP_PUBLIC
   virtual
   ~NodeBaseInterface() = default;
@@ -122,19 +124,11 @@ public:
   bool
   callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
 
-  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.
-   */
+  /// Return list of callback groups associated with this node.
   RCLCPP_PUBLIC
   virtual
-  void
-  for_each_callback_group(const CallbackGroupFunction & func) = 0;
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const = 0;
 
   /// Return the atomic bool which is used to ensure only one executor is used.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

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

rclcpp/include/rclcpp/node_options.hpp

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

rclcpp/include/rclcpp/parameter_event_handler.hpp

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

rclcpp/include/rclcpp/parameter_map.hpp

@@ -41,6 +41,16 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);
 
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
+
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value

rclcpp/include/rclcpp/parameter_value.hpp

@@ -301,16 +301,6 @@ 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<
@@ -321,16 +311,6 @@ 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

@@ -15,32 +15,28 @@
 #ifndef RCLCPP__PUBLISHER_HPP_
 #define RCLCPP__PUBLISHER_HPP_
 
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
+
 #include <functional>
 #include <iostream>
 #include <memory>
 #include <sstream>
 #include <string>
-#include <type_traits>
 #include <utility>
 
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
-#include "rmw/error_handling.h"
-#include "rmw/rmw.h"
-#include "rosidl_runtime_cpp/traits.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/detail/resolve_use_intra_process.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
-#include "rclcpp/get_message_type_support_handle.hpp"
-#include "rclcpp/is_ros_compatible_type.hpp"
 #include "rclcpp/loaned_message.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/publisher_options.hpp"
-#include "rclcpp/type_adapter.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -53,62 +49,15 @@ template<typename MessageT, typename AllocatorT>
 class LoanedMessage;
 
 /// A publisher publishes messages of any type to a topic.
-/**
- * MessageT must be a:
- *
- * - ROS message type with its own type support (e.g. std_msgs::msgs::String), or a
- * - rclcpp::TypeAdapter<CustomType, ROSMessageType>
- *   (e.g. rclcpp::TypeAdapter<std::string, std_msgs::msg::String), or a
- * - custom type that has been setup as the implicit type for a ROS type using
- *   RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(custom_type, ros_message_type)
- *
- * In the case that MessageT is ROS message type (e.g. std_msgs::msg::String),
- * both PublishedType and ROSMessageType will be that type.
- * In the case that MessageT is a TypeAdapter<CustomType, ROSMessageType> type
- * (e.g. TypeAdapter<std::string, std_msgs::msg::String>), PublishedType will
- * be the custom type, and ROSMessageType will be the ros message type.
- *
- * This is achieved because of the "identity specialization" for TypeAdapter,
- * which returns itself if it is already a TypeAdapter, and the default
- * specialization which allows ROSMessageType to be void.
- * \sa rclcpp::TypeAdapter for more details.
- */
 template<typename MessageT, typename AllocatorT = std::allocator<void>>
 class Publisher : public PublisherBase
 {
 public:
-  static_assert(
-    rclcpp::is_ros_compatible_type<MessageT>::value,
-    "given message type is not compatible with ROS and cannot be used with a Publisher");
-
-  /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT.
-  using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
-  /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT.
-  using ROSMessageType = typename rclcpp::TypeAdapter<MessageT>::ros_message_type;
-
-  using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, AllocatorT>;
-  using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
-  using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
-
-  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>;
-  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
-  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
-
-  using MessageAllocatorTraits
-  [[deprecated("use PublishedTypeAllocatorTraits")]] =
-    PublishedTypeAllocatorTraits;
-  using MessageAllocator
-  [[deprecated("use PublishedTypeAllocator")]] =
-    PublishedTypeAllocator;
-  using MessageDeleter
-  [[deprecated("use PublishedTypeDeleter")]] =
-    PublishedTypeDeleter;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<PublishedType, PublishedTypeDeleter>")]] =
-    std::unique_ptr<PublishedType, PublishedTypeDeleter>;
-  using MessageSharedPtr
-  [[deprecated("use std::shared_ptr<const PublishedType>")]] =
-    std::shared_ptr<const PublishedType>;
+  using MessageAllocatorTraits = allocator::AllocRebind<MessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAllocator, MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
@@ -131,14 +80,12 @@ public:
   : PublisherBase(
       node_base,
       topic,
-      rclcpp::get_message_type_support_handle<MessageT>(),
+      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
       options.template to_rcl_publisher_options<MessageT>(qos)),
     options_(options),
-    published_type_allocator_(*options.get_allocator()),
-    ros_message_type_allocator_(*options.get_allocator())
+    message_allocator_(new MessageAllocator(*options.get_allocator().get()))
   {
-    allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_);
-    allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
 
     if (options_.event_callbacks.deadline_callback) {
       this->add_event_handler(
@@ -188,15 +135,15 @@ public:
       // Get the intra process manager instance for this context.
       auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
       // Register the publisher with the intra process manager.
-      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
+      if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication is not allowed with keep all history qos policy");
       }
-      if (qos.depth() == 0) {
+      if (qos.get_rmw_qos_profile().depth == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with a zero qos history depth value");
       }
-      if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) {
+      if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
@@ -223,39 +170,27 @@ public:
    * allocator.
    * \sa rclcpp::LoanedMessage for details of the LoanedMessage class.
    *
-   * \return LoanedMessage containing memory for a ROS message of type ROSMessageType
+   * \return LoanedMessage containing memory for a ROS message of type MessageT
    */
-  rclcpp::LoanedMessage<ROSMessageType, AllocatorT>
+  rclcpp::LoanedMessage<MessageT, AllocatorT>
   borrow_loaned_message()
   {
-    return rclcpp::LoanedMessage<ROSMessageType, AllocatorT>(
-      *this,
-      this->get_ros_message_type_allocator());
+    return rclcpp::LoanedMessage<MessageT, AllocatorT>(this, this->get_allocator());
   }
 
-  /// Publish a message on the topic.
+  /// Send a message to the topic for this publisher.
   /**
-   * This signature is enabled if the element_type of the std::unique_ptr is
-   * a ROS message type, as opposed to the custom_type of a TypeAdapter, and
-   * that type matches the type given when creating the publisher.
-   *
-   * This signature allows the user to give ownership of the message to rclcpp,
-   * allowing for more efficient intra-process communication optimizations.
-   *
-   * \param[in] msg A unique pointer to the message to send.
+   * This function is templated on the input message type, MessageT.
+   * \param[in] msg A shared pointer to the message to send.
    */
-  template<typename T>
-  typename std::enable_if_t<
-    rosidl_generator_traits::is_message<T>::value &&
-    std::is_same<T, ROSMessageType>::value
-  >
-  publish(std::unique_ptr<T, ROSMessageTypeDeleter> msg)
+  virtual void
+  publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     if (!intra_process_is_enabled_) {
       this->do_inter_process_publish(*msg);
       return;
     }
-    // If an interprocess subscription exists, then the unique_ptr is promoted
+    // If an interprocess subscription exist, then the unique_ptr is promoted
     // to a shared_ptr and published.
     // This allows doing the intraprocess publish first and then doing the
     // interprocess publish, resulting in lower publish-to-subscribe latency.
@@ -265,32 +200,15 @@ public:
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared<T, ROSMessageTypeDeleter>(
-        std::move(msg));
+      auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));
       this->do_inter_process_publish(*shared_msg);
     } else {
-      this->do_intra_process_publish<T, ROSMessageTypeDeleter>(std::move(msg));
+      this->do_intra_process_publish(std::move(msg));
     }
   }
 
-  /// Publish a message on the topic.
-  /**
-   * This signature is enabled if the object being published is
-   * a ROS message type, as opposed to the custom_type of a TypeAdapter, and
-   * that type matches the type given when creating the publisher.
-   *
-   * This signature allows the user to give a reference to a message, which is
-   * copied onto the heap without modification so that a copy can be owned by
-   * rclcpp and ownership of the copy can be moved later if needed.
-   *
-   * \param[in] msg A const reference to the message to send.
-   */
-  template<typename T>
-  typename std::enable_if_t<
-    rosidl_generator_traits::is_message<T>::value &&
-    std::is_same<T, ROSMessageType>::value
-  >
-  publish(const T & msg)
+  virtual void
+  publish(const MessageT & msg)
   {
     // Avoid allocating when not using intra process.
     if (!intra_process_is_enabled_) {
@@ -300,84 +218,9 @@ public:
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     // As the message is not const, a copy should be made.
     // A shared_ptr<const MessageT> could also be constructed here.
-    auto unique_msg = this->duplicate_ros_message_as_unique_ptr(msg);
-    this->publish(std::move(unique_msg));
-  }
-
-  /// Publish a message on the topic.
-  /**
-   * This signature is enabled if this class was created with a TypeAdapter and
-   * the element_type of the std::unique_ptr matches the custom_type for the
-   * TypeAdapter used with this class.
-   *
-   * This signature allows the user to give ownership of the message to rclcpp,
-   * allowing for more efficient intra-process communication optimizations.
-   *
-   * \param[in] msg A unique pointer to the message to send.
-   */
-  template<typename T>
-  typename std::enable_if_t<
-    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
-    std::is_same<T, PublishedType>::value
-  >
-  publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
-  {
-    if (!intra_process_is_enabled_) {
-      // If we aren't using intraprocess at all, do the type conversion
-      // immediately and publish interprocess.
-      auto unique_ros_msg = this->create_ros_message_unique_ptr();
-      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *unique_ros_msg);
-      this->do_inter_process_publish(*unique_ros_msg);
-      return;
-    }
-    // If an interprocess subscription exists, then the unique_ptr is promoted
-    // to a shared_ptr and published.
-    // This allows doing the intraprocess publish first and then doing the
-    // interprocess publish, resulting in lower publish-to-subscribe latency.
-    // It's not possible to do that with an unique_ptr,
-    // as do_intra_process_publish takes the ownership of the message.
-    bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
-
-    if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared<T, PublishedTypeDeleter>(
-        std::move(msg));
-      this->do_inter_process_publish(*shared_msg);
-    } else {
-      this->do_intra_process_publish<T, PublishedTypeDeleter>(std::move(msg));
-    }
-  }
-
-  /// Publish a message on the topic.
-  /**
-   * This signature is enabled if this class was created with a TypeAdapter and
-   * the given type matches the custom_type of the TypeAdapter.
-   *
-   * This signature allows the user to give a reference to a message, which is
-   * copied onto the heap without modification so that a copy can be owned by
-   * rclcpp and ownership of the copy can be moved later if needed.
-   *
-   * \param[in] msg A const reference to the message to send.
-   */
-  template<typename T>
-  typename std::enable_if_t<
-    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
-    std::is_same<T, PublishedType>::value
-  >
-  publish(const T & msg)
-  {
-    // Avoid double allocation when not using intra process.
-    if (!intra_process_is_enabled_) {
-      // Convert to the ROS message equivalent and publish it.
-      ROSMessageType ros_msg;
-      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, ros_msg);
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(ros_msg);
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // As the message is not const, a copy should be made.
-    // A shared_ptr<const MessageT> could also be constructed here.
-    auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg);
+    auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg);
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
     this->publish(std::move(unique_msg));
   }
 
@@ -402,7 +245,7 @@ public:
    * \param loaned_msg The LoanedMessage instance to be published.
    */
   void
-  publish(rclcpp::LoanedMessage<ROSMessageType, AllocatorT> && loaned_msg)
+  publish(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg)
   {
     if (!loaned_msg.is_valid()) {
       throw std::runtime_error("loaned message is not valid");
@@ -428,30 +271,20 @@ public:
     }
   }
 
-  [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]]
-  std::shared_ptr<PublishedTypeAllocator>
+  std::shared_ptr<MessageAllocator>
   get_allocator() const
   {
-    return std::make_shared<PublishedTypeAllocator>(published_type_allocator_);
-  }
-
-  PublishedTypeAllocator
-  get_published_type_allocator() const
-  {
-    return published_type_allocator_;
-  }
-
-  ROSMessageTypeAllocator
-  get_ros_message_type_allocator() const
-  {
-    return ros_message_type_allocator_;
+    return message_allocator_;
   }
 
 protected:
   void
-  do_inter_process_publish(const ROSMessageType & msg)
+  do_inter_process_publish(const MessageT & msg)
   {
-    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
+    TRACEPOINT(
+      rclcpp_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      static_cast<const void *>(&msg));
     auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -483,8 +316,7 @@ protected:
   }
 
   void
-  do_loaned_message_publish(
-    std::unique_ptr<ROSMessageType, std::function<void(ROSMessageType *)>> msg)
+  do_loaned_message_publish(std::unique_ptr<MessageT, std::function<void(MessageT *)>> msg)
   {
     auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr);
 
@@ -503,9 +335,8 @@ protected:
     }
   }
 
-  template<typename T, typename DeleterT>
   void
-  do_intra_process_publish(std::unique_ptr<T, DeleterT> msg)
+  do_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -516,17 +347,14 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    ipm->template do_intra_process_publish<T, DeleterT, MessageT, AllocatorT>(
+    ipm->template do_intra_process_publish<MessageT, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_,
-      ros_message_type_deleter_,
-      published_type_allocator_);
+      message_allocator_);
   }
 
-  template<typename T, typename DeleterT>
-  std::shared_ptr<const ROSMessageType>
-  do_intra_process_publish_and_return_shared(std::unique_ptr<T, DeleterT> msg)
+  std::shared_ptr<const MessageT>
+  do_intra_process_publish_and_return_shared(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -537,40 +365,10 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    return ipm->template do_intra_process_publish_and_return_shared<T, DeleterT, MessageT,
-             AllocatorT>(
+    return ipm->template do_intra_process_publish_and_return_shared<MessageT, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_,
-      ros_message_type_deleter_,
-      published_type_allocator_);
-  }
-
-  /// Return a new unique_ptr using the ROSMessageType of the publisher.
-  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
-  create_ros_message_unique_ptr()
-  {
-    auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
-    ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr);
-    return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
-  }
-
-  /// Duplicate a given ROS message as a unique_ptr.
-  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
-  duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg)
-  {
-    auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
-    ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, msg);
-    return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
-  }
-
-  /// Duplicate a given type adapted message as a unique_ptr.
-  std::unique_ptr<PublishedType, PublishedTypeDeleter>
-  duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg)
-  {
-    auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1);
-    PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg);
-    return std::unique_ptr<PublishedType, PublishedTypeDeleter>(ptr, published_type_deleter_);
+      message_allocator_);
   }
 
   /// Copy of original options passed during construction.
@@ -580,10 +378,9 @@ protected:
    */
   const rclcpp::PublisherOptionsWithAllocator<AllocatorT> options_;
 
-  PublishedTypeAllocator published_type_allocator_;
-  PublishedTypeDeleter published_type_deleter_;
-  ROSMessageTypeAllocator ros_message_type_allocator_;
-  ROSMessageTypeDeleter ros_message_type_deleter_;
+  std::shared_ptr<MessageAllocator> message_allocator_;
+
+  MessageDeleter message_deleter_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_options.hpp

@@ -17,7 +17,6 @@
 
 #include <memory>
 #include <string>
-#include <type_traits>
 #include <vector>
 
 #include "rcl/publisher.h"
@@ -65,10 +64,6 @@ struct PublisherOptionsBase
 template<typename Allocator>
 struct PublisherOptionsWithAllocator : public PublisherOptionsBase
 {
-  static_assert(
-    std::is_void_v<typename std::allocator_traits<Allocator>::value_type>,
-    "Publisher allocator value type must be void");
-
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
@@ -85,7 +80,10 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
     rcl_publisher_options_t result = rcl_publisher_get_default_options();
-    result.allocator = this->get_rcl_allocator();
+    using AllocatorTraits = std::allocator_traits<Allocator>;
+    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
+    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_publisher_options.require_unique_network_flow_endpoints =
       this->require_unique_network_flow_endpoints;
@@ -104,35 +102,10 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      if (!allocator_storage_) {
-        allocator_storage_ = std::make_shared<Allocator>();
-      }
-      return allocator_storage_;
+      return std::make_shared<Allocator>();
     }
     return this->allocator;
   }
-
-private:
-  using PlainAllocator =
-    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
-
-  rcl_allocator_t
-  get_rcl_allocator() const
-  {
-    if (!plain_allocator_storage_) {
-      plain_allocator_storage_ =
-        std::make_shared<PlainAllocator>(*this->get_allocator());
-    }
-    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
-  }
-
-  // This is a temporal workaround, to make sure that get_allocator()
-  // always returns a copy of the same allocator.
-  mutable std::shared_ptr<Allocator> allocator_storage_;
-
-  // This is a temporal workaround, to keep the plain allocator that backs
-  // up the rcl allocator returned in rcl_publisher_options_t alive.
-  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/rclcpp.hpp

@@ -140,6 +140,7 @@
  *   - 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,10 +19,6 @@
 #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/serialized_message.hpp

@@ -47,7 +47,7 @@ public:
    * \param[in] initial_capacity The amount of memory to be allocated.
    * \param[in] allocator The allocator to be used for the initialization.
    */
-  explicit SerializedMessage(
+  SerializedMessage(
     size_t initial_capacity,
     const rcl_allocator_t & allocator = rcl_get_default_allocator());
 

rclcpp/include/rclcpp/service.hpp

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

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -388,11 +388,6 @@ 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;
@@ -400,7 +395,7 @@ public:
         timer_handles_.erase(it);
         return;
       }
-      // Else, the timer is no longer valid, remove it and continue
+      // Else, the service is no longer valid, remove it and continue
       it = timer_handles_.erase(it);
     }
   }

rclcpp/include/rclcpp/subscription.hpp

@@ -60,16 +60,10 @@ class NodeTopicsInterface;
 
 /// Subscription implementation, templated on the type of message this subscription receives.
 template<
-  typename MessageT,
+  typename CallbackMessageT,
   typename AllocatorT = std::allocator<void>,
-  /// MessageT::custom_type if MessageT is a TypeAdapter,
-  /// otherwise just MessageT.
-  typename SubscribedT = typename rclcpp::TypeAdapter<MessageT>::custom_type,
-  /// MessageT::ros_message_type if MessageT is a TypeAdapter,
-  /// otherwise just MessageT.
-  typename ROSMessageT = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
   typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    ROSMessageT,
+    CallbackMessageT,
     AllocatorT
   >>
 class Subscription : public SubscriptionBase
@@ -77,36 +71,14 @@ class Subscription : public SubscriptionBase
   friend class rclcpp::node_interfaces::NodeTopicsInterface;
 
 public:
-  // Redeclare these here to use outside of the class.
-  using SubscribedType = SubscribedT;
-  using ROSMessageType = ROSMessageT;
-  using MessageMemoryStrategyType = MessageMemoryStrategyT;
-
-  using SubscribedTypeAllocatorTraits = allocator::AllocRebind<SubscribedType, AllocatorT>;
-  using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type;
-  using SubscribedTypeDeleter = allocator::Deleter<SubscribedTypeAllocator, SubscribedType>;
-
-  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>;
-  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
-  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
-
-  using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] =
-    ROSMessageTypeAllocatorTraits;
-  using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] =
-    ROSMessageTypeAllocator;
-  using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] =
-    ROSMessageTypeDeleter;
-
-  using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr<const ROSMessageType>;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> instead")]] =
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
-
-private:
+  using MessageAllocatorTraits = allocator::AllocRebind<CallbackMessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAllocator, CallbackMessageT>;
+  using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
+  using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
   using SubscriptionTopicStatisticsSharedPtr =
-    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>;
+    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>;
 
-public:
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
   /// Default constructor.
@@ -132,7 +104,7 @@ public:
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rclcpp::QoS & qos,
-    AnySubscriptionCallback<MessageT, AllocatorT> callback,
+    AnySubscriptionCallback<CallbackMessageT, AllocatorT> callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
     typename MessageMemoryStrategyT::SharedPtr message_memory_strategy,
     SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
@@ -140,8 +112,8 @@ public:
       node_base,
       type_support_handle,
       topic_name,
-      options.template to_rcl_subscription_options<ROSMessageType>(qos),
-      callback.is_serialized_message_callback()),
+      options.template to_rcl_subscription_options<CallbackMessageT>(qos),
+      rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
@@ -183,16 +155,16 @@ public:
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // Check if the QoS is compatible with intra-process.
-      auto qos_profile = get_actual_qos();
-      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
+      rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile();
+      if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication is not allowed with keep all history qos policy");
       }
-      if (qos_profile.depth() == 0) {
+      if (qos_profile.depth == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with 0 depth qos policy");
       }
-      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
+      if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
@@ -269,34 +241,11 @@ public:
    * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
    */
   bool
-  take(ROSMessageType & message_out, rclcpp::MessageInfo & message_info_out)
+  take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out)
   {
     return this->take_type_erased(static_cast<void *>(&message_out), message_info_out);
   }
 
-  /// Take the next message from the inter-process subscription.
-  /**
-   * This verison takes a SubscribedType which is different frmo the
-   * ROSMessageType when a rclcpp::TypeAdapter is in used.
-   *
-   * \sa take(ROSMessageType &, rclcpp::MessageInfo &)
-   */
-  template<typename TakeT>
-  std::enable_if_t<
-    !rosidl_generator_traits::is_message<TakeT>::value &&
-    std::is_same_v<TakeT, SubscribedType>,
-    bool
-  >
-  take(TakeT & message_out, rclcpp::MessageInfo & message_info_out)
-  {
-    ROSMessageType local_message;
-    bool taken = this->take_type_erased(static_cast<void *>(&local_message), message_info_out);
-    if (taken) {
-      rclcpp::TypeAdapter<MessageT>::convert_to_custom(local_message, message_out);
-    }
-    return taken;
-  }
-
   std::shared_ptr<void>
   create_message() override
   {
@@ -323,7 +272,7 @@ public:
       // we should ignore this copy of the message.
       return;
     }
-    auto typed_message = std::static_pointer_cast<ROSMessageType>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
 
     std::chrono::time_point<std::chrono::system_clock> now;
     if (subscription_topic_statistics_) {
@@ -341,25 +290,36 @@ public:
     }
   }
 
-  void
-  handle_serialized_message(
-    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
-    const rclcpp::MessageInfo & message_info) override
-  {
-    // TODO(wjwwood): enable topic statistics for serialized messages
-    any_callback_.dispatch(serialized_message, message_info);
-  }
-
   void
   handle_loaned_message(
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
-    auto typed_message = static_cast<ROSMessageType *>(loaned_message);
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
+    }
+
+    auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
-    auto sptr = std::shared_ptr<ROSMessageType>(
-      typed_message, [](ROSMessageType * msg) {(void) msg;});
+    auto sptr = std::shared_ptr<CallbackMessageT>(
+      typed_message, [](CallbackMessageT * msg) {(void) msg;});
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(sptr, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
   }
 
   /// Return the borrowed message.
@@ -369,7 +329,7 @@ public:
   void
   return_message(std::shared_ptr<void> & message) override
   {
-    auto typed_message = std::static_pointer_cast<ROSMessageType>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
@@ -392,24 +352,21 @@ public:
 private:
   RCLCPP_DISABLE_COPY(Subscription)
 
-  AnySubscriptionCallback<MessageT, AllocatorT> any_callback_;
+  AnySubscriptionCallback<CallbackMessageT, AllocatorT> any_callback_;
   /// Copy of original options passed during construction.
   /**
    * It is important to save a copy of this so that the rmw payload which it
    * may contain is kept alive for the duration of the subscription.
    */
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
-  typename message_memory_strategy::MessageMemoryStrategy<ROSMessageType, AllocatorT>::SharedPtr
+  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, AllocatorT>::SharedPtr
     message_memory_strategy_;
-
   /// Component which computes and publishes topic statistics for this subscriber
   SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
-
   using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-    ROSMessageType,
+    CallbackMessageT,
     AllocatorT,
-    ROSMessageTypeDeleter,
-    MessageT>;
+    typename MessageUniquePtr::deleter_type>;
   std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;
 };
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -107,7 +107,7 @@ public:
   /// Get the actual QoS settings, after the defaults have been determined.
   /**
    * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
-   * can only be resolved after the creation of the subscription, and it
+   * can only be resolved after the creation of the publisher, and it
    * depends on the underlying rmw implementation.
    * If the underlying setting in use can't be represented in ROS terms,
    * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
@@ -183,13 +183,6 @@ public:
   void
   handle_message(std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) = 0;
 
-  RCLCPP_PUBLIC
-  virtual
-  void
-  handle_serialized_message(
-    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
-    const rclcpp::MessageInfo & message_info) = 0;
-
   RCLCPP_PUBLIC
   virtual
   void

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -25,14 +25,14 @@
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 
 #include "rclcpp/any_subscription_callback.hpp"
-#include "rclcpp/get_message_type_support_handle.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
-#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
 
 namespace rclcpp
 {
@@ -74,23 +74,26 @@ template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT,
-  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
-  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
-  typename ROSMessageType = typename SubscriptionT::ROSMessageType
->
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >>
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
   typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
-  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
   subscription_topic_stats = nullptr
 )
 {
   auto allocator = options.get_allocator();
 
   using rclcpp::AnySubscriptionCallback;
-  AnySubscriptionCallback<MessageT, AllocatorT> any_subscription_callback(*allocator);
+  AnySubscriptionCallback<CallbackMessageT, AllocatorT> any_subscription_callback(*allocator);
   any_subscription_callback.set(std::forward<CallbackT>(callback));
 
   SubscriptionFactory factory {
@@ -104,9 +107,9 @@ create_subscription_factory(
       using rclcpp::Subscription;
       using rclcpp::SubscriptionBase;
 
-      auto sub = Subscription<MessageT, AllocatorT>::make_shared(
+      auto sub = Subscription<CallbackMessageT, AllocatorT>::make_shared(
         node_base,
-        rclcpp::get_message_type_support_handle<MessageT>(),
+        *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
         topic_name,
         qos,
         any_subscription_callback,

rclcpp/include/rclcpp/subscription_options.hpp

@@ -18,7 +18,6 @@
 #include <chrono>
 #include <memory>
 #include <string>
-#include <type_traits>
 #include <vector>
 
 #include "rclcpp/callback_group.hpp"
@@ -87,10 +86,6 @@ struct SubscriptionOptionsBase
 template<typename Allocator>
 struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
 {
-  static_assert(
-    std::is_void_v<typename std::allocator_traits<Allocator>::value_type>,
-    "Subscription allocator value type must be void");
-
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
@@ -112,7 +107,10 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
-    result.allocator = this->get_rcl_allocator();
+    using AllocatorTraits = std::allocator_traits<Allocator>;
+    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
+    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
     result.rmw_subscription_options.require_unique_network_flow_endpoints =
@@ -126,39 +124,15 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     return result;
   }
 
+  /// Get the allocator, creating one if needed.
   std::shared_ptr<Allocator>
   get_allocator() const
   {
     if (!this->allocator) {
-      if (!allocator_storage_) {
-        allocator_storage_ = std::make_shared<Allocator>();
-      }
-      return allocator_storage_;
+      return std::make_shared<Allocator>();
     }
     return this->allocator;
   }
-
-private:
-  using PlainAllocator =
-    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
-
-  rcl_allocator_t
-  get_rcl_allocator() const
-  {
-    if (!plain_allocator_storage_) {
-      plain_allocator_storage_ =
-        std::make_shared<PlainAllocator>(*this->get_allocator());
-    }
-    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
-  }
-
-  // This is a temporal workaround, to make sure that get_allocator()
-  // always returns a copy of the same allocator.
-  mutable std::shared_ptr<Allocator> allocator_storage_;
-
-  // This is a temporal workaround, to keep the plain allocator that backs
-  // up the rcl allocator returned in rcl_subscription_options_t alive.
-  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/time_source.hpp

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

rclcpp/include/rclcpp/timer.hpp

@@ -91,17 +91,6 @@ 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
@@ -202,29 +191,20 @@ public:
     cancel();
   }
 
-  /**
-   * \sa rclcpp::TimerBase::call
-   * \throws std::runtime_error if it failed to notify timer that callback will occurr
-   */
-  bool
-  call() override
-  {
-    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
-    if (ret == RCL_RET_TIMER_CANCELED) {
-      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
+   * \throws std::runtime_error if it failed to notify timer that callback occurred
    */
   void
   execute_callback() override
   {
+    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
+    if (ret == RCL_RET_TIMER_CANCELED) {
+      return;
+    }
+    if (ret != RCL_RET_OK) {
+      throw std::runtime_error("Failed to notify timer that callback occurred");
+    }
     TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
     TRACEPOINT(callback_end, static_cast<const void *>(&callback_));

rclcpp/include/rclcpp/type_adapter.hpp

@@ -1,201 +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__TYPE_ADAPTER_HPP_
-#define RCLCPP__TYPE_ADAPTER_HPP_
-
-#include <type_traits>
-
-namespace rclcpp
-{
-
-/// Template structure used to adapt custom, user-defined types to ROS types.
-/**
- * Adapting a custom, user-defined type to a ROS type allows that custom type
- * to be used when publishing and subscribing in ROS.
- *
- * In order to adapt a custom type to a ROS type, the user must create a
- * template specialization of this structure for the custom type.
- * In that specialization they must:
- *
- *   - change `is_specialized` to `std::true_type`,
- *   - specify the custom type with `using custom_type = ...`,
- *   - specify the ROS type with `using ros_message_type = ...`,
- *   - provide static convert functions with the signatures:
- *     - static void convert_to_ros(const custom_type &, ros_message_type &)
- *     - static void convert_to_custom(const ros_message_type &, custom_type &)
- *
- * The convert functions must convert from one type to the other.
- *
- * For example, here is a theoretical example for adapting `std::string` to the
- * `std_msgs::msg::String` ROS message type:
- *
- *     template<>
- *     struct rclcpp::TypeAdapter<std::string, std_msgs::msg::String>
- *     {
- *       using is_specialized = std::true_type;
- *       using custom_type = std::string;
- *       using ros_message_type = std_msgs::msg::String;
- *
- *       static
- *       void
- *       convert_to_ros_message(
- *         const custom_type & source,
- *         ros_message_type & destination)
- *       {
- *         destination.data = source;
- *       }
- *
- *       static
- *       void
- *       convert_to_custom(
- *         const ros_message_type & source,
- *         custom_type & destination)
- *       {
- *         destination = source.data;
- *       }
- *     };
- *
- * The adapter can then be used when creating a publisher or subscription,
- * e.g.:
- *
- *     using MyAdaptedType = TypeAdapter<std::string, std_msgs::msg::String>;
- *     auto pub = node->create_publisher<MyAdaptedType>("topic", 10);
- *     auto sub = node->create_subscription<MyAdaptedType>(
- *       "topic",
- *       10,
- *       [](const std::string & msg) {...});
- *
- * You can also be more declarative by using the adapt_type::as metafunctions,
- * which are a bit less ambiguous to read:
- *
- *     using AdaptedType = rclcpp::adapt_type<std::string>::as<std_msgs::msg::String>;
- *     auto pub = node->create_publisher<AdaptedType>(...);
- *
- * If you wish, you may associate a custom type with a single ROS message type,
- * allowing you to be a bit more brief when creating entities, e.g.:
- *
- *     // First you must declare the association, this is similar to how you
- *     // would avoid using the namespace in C++ by doing `using std::vector;`.
- *     RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String);
- *
- *     // Then you can create things with just the custom type, and the ROS
- *     // message type is implied based on the previous statement.
- *     auto pub = node->create_publisher<std::string>(...);
- */
-template<typename CustomType, typename ROSMessageType = void, class Enable = void>
-struct TypeAdapter
-{
-  using is_specialized = std::false_type;
-  using custom_type = CustomType;
-  // In this case, the CustomType is the only thing given, or there is no specialization.
-  // Assign ros_message_type to CustomType for the former case.
-  using ros_message_type = CustomType;
-};
-
-/// Helper template to determine if a type is a TypeAdapter, false specialization.
-template<typename T>
-struct is_type_adapter : std::false_type {};
-
-/// Helper template to determine if a type is a TypeAdapter, true specialization.
-template<typename ... Ts>
-struct is_type_adapter<TypeAdapter<Ts...>>: std::true_type {};
-
-/// Identity specialization for TypeAdapter.
-template<typename T>
-struct TypeAdapter<T, void, std::enable_if_t<is_type_adapter<T>::value>>: T {};
-
-namespace detail
-{
-
-template<typename CustomType, typename ROSMessageType>
-struct assert_type_pair_is_specialized_type_adapter
-{
-  using type_adapter = TypeAdapter<CustomType, ROSMessageType>;
-  static_assert(
-    type_adapter::is_specialized::value,
-    "No type adapter for this custom type/ros message type pair");
-};
-
-}  // namespace detail
-
-/// Template metafunction that can make the type being adapted explicit.
-template<typename CustomType>
-struct adapt_type
-{
-  template<typename ROSMessageType>
-  using as = typename ::rclcpp::detail::assert_type_pair_is_specialized_type_adapter<
-    CustomType,
-    ROSMessageType
-    >::type_adapter;
-};
-
-/// Implicit type adapter used as a short hand way to create something with just the custom type.
-/**
- * This is used when creating a publisher or subscription using just the custom
- * type in conjunction with RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE().
- * For example:
- *
- *     #include "type_adapter_for_std_string_to_std_msgs_String.hpp"
- *
- *     RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String);
- *
- *     int main(...) {
- *       // ...
- *       auto pub = node->create_publisher<std::string>(...);
- *     }
- *
- * \sa TypeAdapter for more examples.
- */
-template<typename CustomType>
-struct ImplicitTypeAdapter
-{
-  using is_specialized = std::false_type;
-};
-
-/// Specialization of TypeAdapter for ImplicitTypeAdapter.
-/**
- * This allows for things like this:
- *
- *    RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String);
- *    auto pub = node->create_publisher<std::string>("topic", 10);
- *
- */
-template<typename T>
-struct TypeAdapter<T, void, std::enable_if_t<ImplicitTypeAdapter<T>::is_specialized::value>>
-  : ImplicitTypeAdapter<T>
-{};
-
-/// Assigns the custom type implicitly to the given custom type/ros message type pair.
-/**
- * Note: this macro needs to be used in the root namespace.
- * We cannot use ::rclcpp to protect against this, due to how GCC interprets the
- * syntax, see: https://stackoverflow.com/a/2781537
- *
- * \sa TypeAdapter
- * \sa ImplicitTypeAdapter
- */
-#define RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CustomType, ROSMessageType) \
-  template<> \
-  struct rclcpp::ImplicitTypeAdapter<CustomType> \
-    : public rclcpp::TypeAdapter<CustomType, ROSMessageType> \
-  { \
-    static_assert( \
-      is_specialized::value, \
-      "Cannot use custom type as ros type when there is no TypeAdapter for that pair"); \
-  }
-
-}  // namespace rclcpp
-
-#endif  // RCLCPP__TYPE_ADAPTER_HPP_

rclcpp/include/rclcpp/utilities.hpp

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

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -90,7 +90,7 @@ bool wait_for_message(
   const std::string & topic,
   std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
 {
-  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<const MsgT>) {});
+  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);
 }

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();
-    RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();});
+    RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();});
 
     // this method comes from the SynchronizationPolicy
     return this->template sync_wait<WaitResult<WaitSetTemplate>>(

rclcpp/package.xml

@@ -2,17 +2,15 @@
 <?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>13.1.0</version>
+  <version>9.2.2</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
-  <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
-  <buildtool_depend>python3</buildtool_depend>
 
   <build_depend>ament_index_cpp</build_depend>
   <build_depend>builtin_interfaces</build_depend>
@@ -39,6 +37,7 @@
   <depend>tracetools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rclcpp/src/rclcpp/clock.cpp

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

rclcpp/src/rclcpp/context.cpp

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

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

@@ -0,0 +1,104 @@
+// 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,22 +47,17 @@ rclcpp::detail::resolve_parameter_overrides(
       rclcpp::exceptions::throw_from_rcl_error(ret);
     }
     if (params) {
-      auto cleanup_params = rcpputils::make_scope_exit(
+      auto cleanup_params = make_scope_exit(
         [params]() {
           rcl_yaml_node_struct_fini(params);
         });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());
 
-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            result[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
+      if (initial_map.count(node_fqn) > 0) {
+        // Combine parameter yaml files, overwriting values in older ones
+        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
+          result[param.get_name()] =
+            rclcpp::ParameterValue(param.get_value_message());
         }
       }
     }

rclcpp/src/rclcpp/executor.cpp

@@ -22,19 +22,18 @@
 
 #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 "rclcpp/node_interfaces/node_base.hpp"
 
 #include "rcutils/logging_macros.h"
 
-#include "tracetools/tracetools.h"
-
 using namespace std::chrono_literals;
 
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -192,7 +191,8 @@ Executor::add_callback_groups_from_nodes_associated_to_executor()
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
-      node->for_each_callback_group(
+      rclcpp::node_interfaces::global_for_each_callback_group(
+        node.get(),
         [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
         {
           if (
@@ -268,12 +268,11 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   // If the node already has an executor
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   if (has_executor.exchange(true)) {
-    throw std::runtime_error(
-            std::string("Node '") + node_ptr->get_fully_qualified_name() +
-            "' has already been added to an executor.");
+    throw std::runtime_error("Node has already been added to an executor.");
   }
   std::lock_guard<std::mutex> guard{mutex_};
-  node_ptr->for_each_callback_group(
+  rclcpp::node_interfaces::global_for_each_callback_group(
+    node_ptr.get(),
     [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr)
     {
       if (!group_ptr->get_associated_with_executor_atomic().load() &&
@@ -452,7 +451,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");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   bool work_available = false;
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
@@ -486,7 +485,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_once() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   spin_once_impl(timeout);
 }
 
@@ -517,15 +516,9 @@ 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) {
@@ -599,7 +592,8 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
       [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
       [&]()
       {
-        subscription->handle_serialized_message(serialized_msg, message_info);
+        auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+        subscription->handle_message(void_serialized_msg, message_info);
       });
     subscription->return_serialized_message(serialized_msg);
   } else if (subscription->can_loan_messages()) {
@@ -686,7 +680,6 @@ 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_);
 
@@ -836,7 +829,6 @@ 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 "rcpputils/scope_exit.hpp"
-
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/scope_exit.hpp"
 
+using rclcpp::detail::MutexTwoPriorities;
 using rclcpp::executors::MultiThreadedExecutor;
 
 MultiThreadedExecutor::MultiThreadedExecutor(
@@ -48,11 +48,12 @@ MultiThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
-    std::lock_guard wait_lock{wait_mutex_};
+    auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
+    std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
       auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
       threads.emplace_back(func);
@@ -77,13 +78,26 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      std::lock_guard wait_lock{wait_mutex_};
+      auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
+      std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_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();
@@ -91,6 +105,14 @@ 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,10 +12,9 @@
 // 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;
 
@@ -30,7 +29,7 @@ SingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_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

@@ -23,6 +23,7 @@
 
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
 
 using rclcpp::executors::StaticExecutorEntitiesCollector;
 
@@ -61,7 +62,8 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 void
 StaticExecutorEntitiesCollector::init(
   rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
+  rcl_guard_condition_t * executor_guard_condition)
 {
   // Empty initialize executable list
   exec_list_ = rclcpp::experimental::ExecutableList();
@@ -73,6 +75,9 @@ 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);
@@ -104,14 +109,6 @@ 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
@@ -281,8 +278,7 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
 {
-  std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-  return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
+  return weak_nodes_to_guard_conditions_.size();
 }
 
 bool
@@ -295,7 +291,8 @@ StaticExecutorEntitiesCollector::add_node(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
-  node_ptr->for_each_callback_group(
+  rclcpp::node_interfaces::global_for_each_callback_group(
+    node_ptr.get(),
     [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
     {
       if (
@@ -334,8 +331,7 @@ StaticExecutorEntitiesCollector::add_callback_group(
     throw std::runtime_error("Callback group was already added to executor.");
   }
   if (is_new_node) {
-    std::lock_guard<std::mutex> guard{new_nodes_mutex_};
-    new_nodes_.push_back(node_ptr);
+    weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition();
     return true;
   }
   return false;
@@ -475,7 +471,8 @@ StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_ex
   for (const auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
-      node->for_each_callback_group(
+      rclcpp::node_interfaces::global_for_each_callback_group(
+        node.get(),
         [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
         {
           if (shared_group_ptr->automatically_add_to_executor_with_node() &&

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -16,10 +16,9 @@
 
 #include <chrono>
 #include <memory>
-#include <utility>
 #include <vector>
 
-#include "rcpputils/scope_exit.hpp"
+#include "rclcpp/scope_exit.hpp"
 
 using rclcpp::executors::StaticSingleThreadedExecutor;
 using rclcpp::experimental::ExecutableList;
@@ -44,11 +43,11 @@ StaticSingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_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_);
+  entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work
@@ -82,7 +81,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_);
+    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
   }
 
   auto start = std::chrono::steady_clock::now();
@@ -101,7 +100,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
 
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
     // Get executables that are ready now
@@ -119,7 +118,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_);
+    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
   }
 
   if (rclcpp::ok(context_) && spinning.load()) {
@@ -238,9 +237,7 @@ 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()) {
-        auto timer = entities_collector_->get_timer(i);
-        timer->call();
-        execute_timer(std::move(timer));
+        execute_timer(entities_collector_->get_timer(i));
         if (spin_once) {
           return true;
         }

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -19,6 +19,7 @@
 #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/generic_subscription.cpp

@@ -36,19 +36,11 @@ std::shared_ptr<rclcpp::SerializedMessage> GenericSubscription::create_serialize
 }
 
 void GenericSubscription::handle_message(
-  std::shared_ptr<void> &,
-  const rclcpp::MessageInfo &)
+  std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info)
 {
-  throw rclcpp::exceptions::UnimplementedError(
-          "handle_message is not implemented for GenericSubscription");
-}
-
-void
-GenericSubscription::handle_serialized_message(
-  const std::shared_ptr<rclcpp::SerializedMessage> & message,
-  const rclcpp::MessageInfo &)
-{
-  callback_(message);
+  (void) message_info;
+  auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
+  callback_(typed_message);
 }
 
 void GenericSubscription::handle_loaned_message(

rclcpp/src/rclcpp/init_options.cpp

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

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -36,26 +36,23 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
-  uint64_t pub_id = IntraProcessManager::get_next_unique_id();
+  auto id = IntraProcessManager::get_next_unique_id();
 
-  publishers_[pub_id] = publisher;
+  publishers_[id].publisher = publisher;
+  publishers_[id].topic_name = publisher->get_topic_name();
+  publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile();
 
   // Initialize the subscriptions storage for this publisher.
-  pub_to_subs_[pub_id] = SplittedSubscriptions();
+  pub_to_subs_[id] = SplittedSubscriptions();
 
   // create an entry for the publisher id and populate with already existing subscriptions
   for (auto & pair : subscriptions_) {
-    auto subscription = pair.second.lock();
-    if (!subscription) {
-      continue;
-    }
-    if (can_communicate(publisher, subscription)) {
-      uint64_t sub_id = pair.first;
-      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
+    if (can_communicate(publishers_[id], pair.second)) {
+      insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method);
     }
   }
 
-  return pub_id;
+  return id;
 }
 
 uint64_t
@@ -63,23 +60,21 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
-  uint64_t sub_id = IntraProcessManager::get_next_unique_id();
+  auto id = IntraProcessManager::get_next_unique_id();
 
-  subscriptions_[sub_id] = subscription;
+  subscriptions_[id].subscription = subscription;
+  subscriptions_[id].topic_name = subscription->get_topic_name();
+  subscriptions_[id].qos = subscription->get_actual_qos();
+  subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method();
 
   // adds the subscription id to all the matchable publishers
   for (auto & pair : publishers_) {
-    auto publisher = pair.second.lock();
-    if (!publisher) {
-      continue;
-    }
-    if (can_communicate(publisher, subscription)) {
-      uint64_t pub_id = pair.first;
-      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
+    if (can_communicate(pair.second, subscriptions_[id])) {
+      insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method);
     }
   }
 
-  return sub_id;
+  return id;
 }
 
 void
@@ -121,7 +116,7 @@ IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
   std::shared_lock<std::shared_timed_mutex> lock(mutex_);
 
   for (auto & publisher_pair : publishers_) {
-    auto publisher = publisher_pair.second.lock();
+    auto publisher = publisher_pair.second.publisher.lock();
     if (!publisher) {
       continue;
     }
@@ -162,7 +157,7 @@ IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subsc
   if (subscription_it == subscriptions_.end()) {
     return nullptr;
   } else {
-    auto subscription = subscription_it->second.lock();
+    auto subscription = subscription_it->second.subscription.lock();
     if (subscription) {
       return subscription;
     } else {
@@ -209,16 +204,25 @@ IntraProcessManager::insert_sub_id_for_pub(
 
 bool
 IntraProcessManager::can_communicate(
-  rclcpp::PublisherBase::SharedPtr pub,
-  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const
+  PublisherInfo pub_info,
+  SubscriptionInfo sub_info) const
 {
   // publisher and subscription must be on the same topic
-  if (strcmp(pub->get_topic_name(), sub->get_topic_name()) != 0) {
+  if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) {
     return false;
   }
 
-  auto check_result = rclcpp::qos_check_compatible(pub->get_actual_qos(), sub->get_actual_qos());
-  if (check_result.compatibility == rclcpp::QoSCompatibility::Error) {
+  // TODO(alsora): the following checks for qos compatibility should be provided by the RMW
+  // a reliable subscription can't be connected with a best effort publisher
+  if (
+    sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE &&
+    pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
+  {
+    return false;
+  }
+
+  // a publisher and a subscription with different durability can't communicate
+  if (sub_info.qos.durability != pub_info.qos.durability) {
     return false;
   }
 

rclcpp/src/rclcpp/node.cpp

@@ -47,9 +47,6 @@ using rclcpp::Node;
 using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
-namespace
-{
-
 RCLCPP_LOCAL
 std::string
 extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
@@ -57,19 +54,19 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
   // Assumption is that the existing_sub_namespace does not need checking
   // because it would be checked already when it was set with this function.
 
-  if (extension.empty()) {
-    throw rclcpp::exceptions::NameValidationError(
-            "sub_namespace",
-            extension.c_str(),
-            "sub-nodes should not extend nodes by an empty sub-namespace",
-            0);
-  } else if (extension.front() == '/') {
-    // check if the new sub-namespace extension is absolute
+  // check if the new sub-namespace extension is absolute
+  if (extension.front() == '/') {
     throw rclcpp::exceptions::NameValidationError(
             "sub_namespace",
             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;
@@ -79,7 +76,7 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
     new_sub_namespace = existing_sub_namespace + "/" + extension;
   }
 
-  // remove any trailing `/` so that new extensions do not result in `//`
+  // remove any trailing `/` so that new extensions do no result in `//`
   if (new_sub_namespace.back() == '/') {
     new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
   }
@@ -107,8 +104,6 @@ create_effective_namespace(const std::string & node_namespace, const std::string
   }
 }
 
-}  // namespace
-
 Node::Node(
   const std::string & node_name,
   const NodeOptions & options)
@@ -237,8 +232,6 @@ Node::Node(
   node_services_(other.node_services_),
   node_clock_(other.node_clock_),
   node_parameters_(other.node_parameters_),
-  node_time_source_(other.node_time_source_),
-  node_waitables_(other.node_waitables_),
   node_options_(other.node_options_),
   sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
   effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
@@ -499,11 +492,10 @@ 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);
 }
 
-void
-Node::for_each_callback_group(
-  const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
+const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+Node::get_callback_groups() const
 {
-  node_base_->for_each_callback_group(func);
+  return node_base_->get_callback_groups();
 }
 
 rclcpp::Event::SharedPtr
@@ -623,3 +615,9 @@ Node::get_node_options() const
 {
   return this->node_options_;
 }
+
+void Node::for_each_callback_group(
+  const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
+{
+  rclcpp::node_interfaces::global_for_each_callback_group(node_base_.get(), func);
+}

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -15,7 +15,6 @@
 #include <string>
 #include <limits>
 #include <memory>
-#include <utility>
 #include <vector>
 
 #include "rclcpp/node_interfaces/node_base.hpp"
@@ -32,92 +31,7 @@ using rclcpp::exceptions::throw_from_rcl_error;
 
 using rclcpp::node_interfaces::NodeBase;
 
-namespace
-{
-/// A class to manage the lifetime of a node handle and its context
-/**
- * This class bundles together the lifetime of the rcl_node_t handle with the
- * lifetime of the RCL context. This ensures that the context will remain alive
- * for as long as the rcl_node_t handle is alive.
- */
-class NodeHandleWithContext
-{
-public:
-  /// Make an instance of a NodeHandleWithContext
-  /**
-   * The make function returns a std::shared_ptr<rcl_node_t> which is actually
-   * an alias for a std::shared_ptr<NodeHandleWithContext>. There is no use for
-   * accessing a NodeHandleWithContext instance directly, because its only job
-   * is to couple together the lifetime of a context with the lifetime of a
-   * node handle that depends on it.
-   */
-  static std::shared_ptr<rcl_node_t>
-  make(
-    rclcpp::Context::SharedPtr context,
-    std::shared_ptr<std::recursive_mutex> logging_mutex,
-    rcl_node_t * node_handle)
-  {
-    auto aliased_ptr = std::make_shared<NodeHandleWithContext>(
-      NodeHandleWithContext(
-        std::move(context),
-        std::move(logging_mutex),
-        node_handle));
-
-    return std::shared_ptr<rcl_node_t>(std::move(aliased_ptr), node_handle);
-  }
-
-  // This class should not be copied. It should only exist in the
-  // std::shared_ptr that it was originally provided in.
-  NodeHandleWithContext(const NodeHandleWithContext &) = delete;
-  NodeHandleWithContext & operator=(const NodeHandleWithContext &) = delete;
-  NodeHandleWithContext & operator=(NodeHandleWithContext &&) = delete;
-
-  NodeHandleWithContext(NodeHandleWithContext && other)
-  : context_(std::move(other.context_)),
-    logging_mutex_(std::move(other.logging_mutex_)),
-    node_handle_(other.node_handle_)
-  {
-    other.node_handle_ = nullptr;
-  }
-
-  ~NodeHandleWithContext()
-  {
-    if (!node_handle_) {
-      // If the node_handle_ is null, then this object was moved-from. We don't
-      // need to do any cleanup.
-      return;
-    }
-
-    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
-    // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
-    // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
-    // here directly.
-    if (rcl_node_fini(node_handle_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
-    }
-    delete node_handle_;
-  }
-
-private:
-  // The constructor is private because this class is only meant to be aliased
-  // into a std::shared_ptr<rcl_node_t> and should not be constructed any other
-  // way.
-  NodeHandleWithContext(
-    rclcpp::Context::SharedPtr context,
-    std::shared_ptr<std::recursive_mutex> logging_mutex,
-    rcl_node_t * node_handle)
-  : context_(std::move(context)),
-    logging_mutex_(std::move(logging_mutex)),
-    node_handle_(node_handle)
-  {}
-
-  rclcpp::Context::SharedPtr context_;
-  std::shared_ptr<std::recursive_mutex> logging_mutex_;
-  rcl_node_t * node_handle_;
-};
-}  // anonymous namespace
+using rclcpp::node_interfaces::map_of_mutexes;
 
 NodeBase::NodeBase(
   const std::string & node_name,
@@ -134,6 +48,9 @@ NodeBase::NodeBase(
   associated_with_executor_(false),
   notify_guard_condition_is_valid_(false)
 {
+  // Generate a mutex for this instance of NodeBase
+  NodeBase::map_object.create_mutex_of_nodebase(this);
+
   // Setup the guard condition that is notified when changes occur in the graph.
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
   rcl_ret_t ret = rcl_guard_condition_init(
@@ -219,7 +136,20 @@ NodeBase::NodeBase(
     throw_from_rcl_error(ret, "failed to initialize rcl node");
   }
 
-  node_handle_ = NodeHandleWithContext::make(context_, logging_mutex, rcl_node.release());
+  node_handle_.reset(
+    rcl_node.release(),
+    [logging_mutex](rcl_node_t * node) -> void {
+      std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+      // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
+      // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
+      // here directly.
+      if (rcl_node_fini(node) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
+      }
+      delete node;
+    });
 
   // Create the default callback group.
   using rclcpp::CallbackGroupType;
@@ -241,6 +171,8 @@ NodeBase::~NodeBase()
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
     }
   }
+
+  NodeBase::map_object.delete_mutex_of_nodebase(this);
 }
 
 const char *
@@ -299,7 +231,8 @@ NodeBase::create_callback_group(
   auto group = std::make_shared<rclcpp::CallbackGroup>(
     group_type,
     automatically_add_to_executor_with_node);
-  std::lock_guard<std::mutex> lock(callback_groups_mutex_);
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(this);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
   callback_groups_.push_back(group);
   return group;
 }
@@ -313,7 +246,9 @@ NodeBase::get_default_callback_group()
 bool
 NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group)
 {
-  std::lock_guard<std::mutex> lock(callback_groups_mutex_);
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(this);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
+
   for (auto & weak_group : this->callback_groups_) {
     auto cur_group = weak_group.lock();
     if (cur_group && (cur_group == group)) {
@@ -323,15 +258,10 @@ NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group)
   return false;
 }
 
-void NodeBase::for_each_callback_group(const CallbackGroupFunction & func)
+const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+NodeBase::get_callback_groups() const
 {
-  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);
-    }
-  }
+  return callback_groups_;
 }
 
 std::atomic_bool &
@@ -388,3 +318,41 @@ NodeBase::resolve_topic_or_service_name(
   allocator.deallocate(output_cstr, allocator.state);
   return output;
 }
+
+map_of_mutexes NodeBase::map_object = map_of_mutexes();
+
+void map_of_mutexes::create_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  this->data_.emplace(nodebase, std::make_shared<std::mutex>() );
+}
+
+std::shared_ptr<std::mutex> map_of_mutexes::get_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  return this->data_[nodebase];
+}
+
+void map_of_mutexes::delete_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  this->data_.erase(nodebase);
+}
+
+// For each callback group free function implementation
+void rclcpp::node_interfaces::global_for_each_callback_group(
+  NodeBaseInterface * node_base_interface, const NodeBaseInterface::CallbackGroupFunction & func)
+{
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(node_base_interface);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
+
+  for (const auto & weak_group : node_base_interface->get_callback_groups()) {
+    auto group = weak_group.lock();
+    if (group) {
+      func(group);
+    }
+  }
+}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

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

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

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

rclcpp/src/rclcpp/parameter_event_handler.cpp

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

rclcpp/src/rclcpp/parameter_map.cpp

@@ -13,8 +13,10 @@
 // limitations under the License.
 
 #include <string>
+#include <regex>
 #include <vector>
 
+#include "rcpputils/find_and_replace.hpp"
 #include "rclcpp/parameter_map.hpp"
 
 using rclcpp::exceptions::InvalidParametersException;
@@ -24,6 +26,12 @@ using rclcpp::ParameterValue;
 
 ParameterMap
 rclcpp::parameter_map_from(const rcl_params_t * const c_params)
+{
+  return parameter_map_from(c_params, nullptr);
+}
+
+ParameterMap
+rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn)
 {
   if (NULL == c_params) {
     throw InvalidParametersException("parameters struct is NULL");
@@ -49,6 +57,17 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
       node_name = c_node_name;
     }
 
+    if (node_fqn) {
+      // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"]
+      std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)");
+      if (!std::regex_match(node_fqn, std::regex(regex))) {
+        // No need to parse the items because the user just care about node_fqn
+        continue;
+      }
+
+      node_name = node_fqn;
+    }
+
     const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
 
     std::vector<Parameter> & params_node = parameters[node_name];
@@ -65,6 +84,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
       params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
     }
   }
+
   return parameters;
 }
 

rclcpp/src/rclcpp/signal_handler.cpp

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

rclcpp/src/rclcpp/signal_handler.hpp

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

rclcpp/src/rclcpp/subscription_base.cpp

@@ -143,7 +143,6 @@ 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/subscription_intra_process_base.cpp

@@ -31,7 +31,7 @@ SubscriptionIntraProcessBase::get_topic_name() const
   return topic_name_.c_str();
 }
 
-rclcpp::QoS
+rmw_qos_profile_t
 SubscriptionIntraProcessBase::get_actual_qos() const
 {
   return qos_profile_;

rclcpp/src/rclcpp/time_source.cpp

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

rclcpp/src/rclcpp/utilities.cpp

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

rclcpp/test/benchmark/benchmark_client.cpp

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

rclcpp/test/benchmark/benchmark_executor.cpp

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

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

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

rclcpp/test/benchmark/benchmark_node.cpp

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

rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp

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

rclcpp/test/benchmark/benchmark_parameter_client.cpp

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

rclcpp/test/benchmark/benchmark_service.cpp

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

rclcpp/test/msg/String.msg

@@ -1 +0,0 @@
-string data

rclcpp/test/rclcpp/CMakeLists.txt

@@ -7,13 +7,10 @@ add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}")
 rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   ../msg/Header.msg
   ../msg/MessageWithHeader.msg
-  ../msg/String.msg
   DEPENDENCIES builtin_interfaces
   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
@@ -358,48 +355,6 @@ if(TARGET test_publisher)
   )
   target_link_libraries(test_publisher ${PROJECT_NAME} mimick)
 endif()
-
-set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-if(WIN32)
-  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-endif()
-
-ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-)
-if(TARGET test_publisher_with_type_adapter)
-  ament_target_dependencies(test_publisher_with_type_adapter
-    "rcutils"
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  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
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-)
-if(TARGET test_subscription_with_type_adapter)
-  ament_target_dependencies(test_subscription_with_type_adapter
-    "rcutils"
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  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)
 if(TARGET test_publisher_subscription_count_api)
   ament_target_dependencies(test_publisher_subscription_count_api
@@ -552,6 +507,11 @@ if(TARGET test_find_weak_nodes)
   target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
 endif()
 
+set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+if(WIN32)
+  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+endif()
+
 ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp)
 ament_target_dependencies(test_externally_defined_services
   "rcl"
@@ -686,9 +646,8 @@ if(TARGET test_subscription_topic_statistics)
     "rosidl_typesupport_cpp"
     "statistics_msgs"
     "test_msgs")
-  target_link_libraries(test_subscription_topic_statistics
-    ${PROJECT_NAME}
-    ${cpp_typesupport_target})
+  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
+  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
 endif()
 
 ament_add_gtest(test_subscription_options test_subscription_options.cpp)

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -20,8 +20,6 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "rcpputils/scope_exit.hpp"
-
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
@@ -140,14 +138,15 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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),
+    entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition),
     std::runtime_error);
 }
 
@@ -164,13 +163,14 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_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);
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
   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,14 +210,15 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_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());
@@ -280,14 +281,15 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   EXPECT_EQ(
     1u + expected_number_of_entities->subscriptions,
@@ -306,7 +308,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);
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
   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());
@@ -359,13 +361,14 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
@@ -389,13 +392,14 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
@@ -426,13 +430,14 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
@@ -473,14 +478,15 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   {
     auto mock = mocking_utils::patch_and_return(
@@ -505,13 +511,14 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   RCLCPP_EXPECT_THROW_EQ(
     entities_collector_->add_to_wait_set(nullptr),
@@ -532,10 +539,11 @@ 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));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
@@ -545,8 +553,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group)
 
   cb_group.reset();
 
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_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()));
@@ -609,13 +617,14 @@ TEST_F(
   EXPECT_EQ(
     RCL_RET_OK,
     rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+  RCLCPP_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);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   cb_group->get_associated_with_executor_atomic().exchange(false);
   std::shared_ptr<void> data = entities_collector_->take_data();

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

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

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -31,6 +31,8 @@
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
+#include "rcpputils/filesystem_helper.hpp"
+
 class TestNodeParameters : public ::testing::Test
 {
 public:
@@ -47,6 +49,7 @@ public:
       dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
       node->get_node_parameters_interface().get());
     ASSERT_NE(nullptr, node_parameters);
+    test_resources_path /= "test_node_parameters";
   }
 
   void TearDown()
@@ -57,6 +60,8 @@ public:
 protected:
   std::shared_ptr<rclcpp::Node> node;
   rclcpp::node_interfaces::NodeParameters * node_parameters;
+
+  rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY};
 };
 
 TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
@@ -199,3 +204,130 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) {
     node_parameters->remove_on_set_parameters_callback(handle.get()),
     std::runtime_error("Callback doesn't exist"));
 }
+
+TEST_F(TestNodeParameters, wildcard_with_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(7u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_one_star").get<std::string>(),
+    "namespace_wild_one_star");
+  EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get<std::string>(), "node_wild_in_ns");
+  EXPECT_EQ(
+    parameter_overrides.at("node_wild_in_ns_another").get<std::string>(),
+    "node_wild_in_ns_another");
+  EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get<std::string>(), "explicit_in_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+}
+
+TEST_F(TestNodeParameters, wildcard_no_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(5u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get<std::string>(), "node_wild_no_ns");
+  EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get<std::string>(), "explicit_no_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+  // "/*" match exactly one token, not expect to get `namespace_wild_one_star`
+  EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u);
+}
+
+TEST_F(TestNodeParameters, params_by_order)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "params_by_order.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(3u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("a_value").get<std::string>(), "last_one_win");
+  EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+  EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+}
+
+TEST_F(TestNodeParameters, complicated_wildcards)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "complicated_wildcards.yaml").string()
+  });
+
+  {
+    // regex matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/d/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(2u, parameter_overrides.size());
+    EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+    EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+  }
+
+  {
+    // regex not matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(0u, parameter_overrides.size());
+  }
+}

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -32,7 +32,6 @@ 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/node_interfaces/test_node_topics.cpp

@@ -71,8 +71,6 @@ public:
 
   void handle_message(std::shared_ptr<void> &, const rclcpp::MessageInfo &) override {}
   void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {}
-  void handle_serialized_message(
-    const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &) override {}
   void return_message(std::shared_ptr<void> &) override {}
   void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> &) override {}
 };

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -21,8 +21,9 @@
 
 #include "gtest/gtest.h"
 
+#include "rclcpp/scope_exit.hpp"
 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
-#include "rcpputils/scope_exit.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
@@ -263,7 +264,7 @@ protected:
              "Calling rcl_wait_set_init() with expected sufficient capacities failed";
     }
 
-    RCPPUTILS_SCOPE_EXIT(
+    RCLCPP_SCOPE_EXIT(
     {
       EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
     });
@@ -291,7 +292,7 @@ protected:
              "Calling rcl_wait_set_init() with expected insufficient capacities failed";
     }
 
-    RCPPUTILS_SCOPE_EXIT(
+    RCLCPP_SCOPE_EXIT(
     {
       EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity));
     });
@@ -376,7 +377,8 @@ 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;
-    basic_node_base->for_each_callback_group(
+    rclcpp::node_interfaces::global_for_each_callback_group(
+      basic_node_base.get(),
       [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
       {
         weak_groups_to_nodes.insert(
@@ -385,7 +387,8 @@ protected:
             group_ptr,
             basic_node_base));
       });
-    node_base->for_each_callback_group(
+    rclcpp::node_interfaces::global_for_each_callback_group(
+      node_base.get(),
       [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
       {
         weak_groups_to_nodes.insert(
@@ -578,7 +581,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));
-  RCPPUTILS_SCOPE_EXIT(
+  RCLCPP_SCOPE_EXIT(
   {
     EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition));
   });

rclcpp/test/rclcpp/test_any_service_callback.cpp

@@ -22,7 +22,6 @@
 #include <utility>
 
 #include "rclcpp/any_service_callback.hpp"
-#include "rclcpp/service.hpp"
 #include "test_msgs/srv/empty.hpp"
 #include "test_msgs/srv/empty.h"
 
@@ -45,7 +44,7 @@ protected:
 
 TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) {
   EXPECT_THROW(
-    any_service_callback_.dispatch(nullptr, request_header_, request_),
+    any_service_callback_.dispatch(request_header_, request_, response_),
     std::runtime_error);
 }
 
@@ -58,7 +57,7 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) {
 
   any_service_callback_.set(callback);
   EXPECT_NO_THROW(
-    EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
+    any_service_callback_.dispatch(request_header_, request_, response_));
   EXPECT_EQ(callback_calls, 1);
 }
 
@@ -74,36 +73,6 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) {
 
   any_service_callback_.set(callback_with_header);
   EXPECT_NO_THROW(
-    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_)));
+    any_service_callback_.dispatch(request_header_, request_, response_));
   EXPECT_EQ(callback_with_header_calls, 1);
 }

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -19,8 +19,6 @@
 #include <string>
 #include <utility>
 
-// TODO(aprotyas): Figure out better way to suppress deprecation warnings.
-#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1
 #include "rclcpp/any_subscription_callback.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/msg/empty.h"
@@ -69,7 +67,7 @@ void construct_with_null_allocator()
 TEST(AnySubscriptionCallback, null_allocator) {
   EXPECT_THROW(
     construct_with_null_allocator(),
-    std::invalid_argument);
+    std::runtime_error);
 }
 
 TEST_F(TestAnySubscriptionCallback, construct_destruct) {
@@ -86,12 +84,10 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
     any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_),
     std::runtime_error);
   EXPECT_THROW(
-    any_subscription_callback_.template dispatch_intra_process<test_msgs::msg::Empty>(
-      msg_shared_ptr_, message_info_),
+    any_subscription_callback_.dispatch_intra_process(msg_shared_ptr_, message_info_),
     std::runtime_error);
   EXPECT_THROW(
-    any_subscription_callback_.template dispatch_intra_process<test_msgs::msg::Empty>(
-      get_unique_ptr_msg(), message_info_),
+    any_subscription_callback_.dispatch_intra_process(get_unique_ptr_msg(), message_info_),
     std::runtime_error);
 }
 
@@ -99,69 +95,44 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
 // Parameterized test to test across all callback types and dispatch types.
 //
 
-// Type adapter to be used in tests.
-struct MyEmpty {};
-
-template<>
-struct rclcpp::TypeAdapter<MyEmpty, test_msgs::msg::Empty>
-{
-  using is_specialized = std::true_type;
-  using custom_type = MyEmpty;
-  using ros_message_type = test_msgs::msg::Empty;
-
-  static
-  void
-  convert_to_ros_message(const custom_type &, ros_message_type &)
-  {}
-
-  static
-  void
-  convert_to_custom(const ros_message_type &, custom_type &)
-  {}
-};
-
-using MyTA = rclcpp::TypeAdapter<MyEmpty, test_msgs::msg::Empty>;
-
-template<typename MessageT>
 class InstanceContextImpl
 {
 public:
   InstanceContextImpl() = default;
   virtual ~InstanceContextImpl() = default;
 
-  explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback<MessageT> asc)
+  explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc)
   : any_subscription_callback_(asc)
   {}
 
   virtual
-  rclcpp::AnySubscriptionCallback<MessageT>
+  rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>
   get_any_subscription_callback_to_test() const
   {
     return any_subscription_callback_;
   }
 
 protected:
-  rclcpp::AnySubscriptionCallback<MessageT> any_subscription_callback_;
+  rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> any_subscription_callback_;
 };
 
-template<typename MessageT>
 class InstanceContext
 {
 public:
-  InstanceContext(const std::string & name, std::shared_ptr<InstanceContextImpl<MessageT>> impl)
+  InstanceContext(const std::string & name, std::shared_ptr<InstanceContextImpl> impl)
   : name(name), impl_(impl)
   {}
 
   InstanceContext(
     const std::string & name,
-    rclcpp::AnySubscriptionCallback<MessageT> asc)
-  : name(name), impl_(std::make_shared<InstanceContextImpl<MessageT>>(asc))
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc)
+  : name(name), impl_(std::make_shared<InstanceContextImpl>(asc))
   {}
 
   InstanceContext(const InstanceContext & other)
   : InstanceContext(other.name, other.impl_) {}
 
-  rclcpp::AnySubscriptionCallback<MessageT>
+  rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>
   get_any_subscription_callback_to_test() const
   {
     return impl_->get_any_subscription_callback_to_test();
@@ -170,17 +141,12 @@ public:
   std::string name;
 
 protected:
-  std::shared_ptr<InstanceContextImpl<MessageT>> impl_;
+  std::shared_ptr<InstanceContextImpl> impl_;
 };
 
 class DispatchTests
   : public TestAnySubscriptionCallback,
-  public ::testing::WithParamInterface<InstanceContext<test_msgs::msg::Empty>>
-{};
-
-class DispatchTestsWithTA
-  : public TestAnySubscriptionCallback,
-  public ::testing::WithParamInterface<InstanceContext<MyTA>>
+  public ::testing::WithParamInterface<InstanceContext>
 {};
 
 auto
@@ -189,69 +155,57 @@ format_parameter(const ::testing::TestParamInfo<DispatchTests::ParamType> & info
   return info.param.name;
 }
 
-auto
-format_parameter_with_ta(const ::testing::TestParamInfo<DispatchTestsWithTA::ParamType> & info)
-{
-  return info.param.name;
+// Testing dispatch with shared_ptr<MessageT> as input
+TEST_P(DispatchTests, test_inter_shared_dispatch) {
+  auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test();
+  any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_);
 }
 
-#define PARAMETERIZED_TESTS(DispatchTests_name) \
-  /* Testing dispatch with shared_ptr<MessageT> as input */ \
-  TEST_P(DispatchTests_name, test_inter_shared_dispatch) { \
-    auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \
-    any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); \
-  } \
- \
-  /* Testing dispatch with shared_ptr<const MessageT> as input */ \
-  TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \
-    auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \
-    any_subscription_callback_to_test.template dispatch_intra_process<test_msgs::msg::Empty>( \
-      msg_shared_ptr_, message_info_); \
-  } \
- \
-  /* Testing dispatch with unique_ptr<MessageT> as input */ \
-  TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \
-    auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \
-    any_subscription_callback_to_test.template dispatch_intra_process<test_msgs::msg::Empty>( \
-      get_unique_ptr_msg(), message_info_); \
-  }
+// Testing dispatch with shared_ptr<const MessageT> as input
+TEST_P(DispatchTests, test_intra_shared_dispatch) {
+  auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test();
+  any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_);
+}
 
-PARAMETERIZED_TESTS(DispatchTests)
-PARAMETERIZED_TESTS(DispatchTestsWithTA)
+// Testing dispatch with unique_ptr<MessageT> as input
+TEST_P(DispatchTests, test_intra_unique_dispatch) {
+  auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test();
+  any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_);
+}
 
 // Generic classes for testing callbacks using std::bind to class methods.
-template<typename MessageT, typename ... CallbackArgs>
-class BindContextImpl : public InstanceContextImpl<MessageT>
+template<typename ... CallbackArgs>
+class BindContextImpl : public InstanceContextImpl
 {
   static constexpr size_t number_of_callback_args{sizeof...(CallbackArgs)};
 
 public:
-  using InstanceContextImpl<MessageT>::InstanceContextImpl;
+  using InstanceContextImpl::InstanceContextImpl;
   virtual ~BindContextImpl() = default;
 
   void on_message(CallbackArgs ...) const {}
 
-  rclcpp::AnySubscriptionCallback<MessageT>
+  rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>
   get_any_subscription_callback_to_test() const override
   {
     if constexpr (number_of_callback_args == 1) {
-      return rclcpp::AnySubscriptionCallback<MessageT>().set(
+      return rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         std::bind(&BindContextImpl::on_message, this, std::placeholders::_1)
       );
     } else {
-      return rclcpp::AnySubscriptionCallback<MessageT>().set(
+      return rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         std::bind(&BindContextImpl::on_message, this, std::placeholders::_1, std::placeholders::_2)
       );
     }
   }
 };
 
-template<typename MessageT, typename ... CallbackArgs>
-class BindContext : public InstanceContext<MessageT>
+template<typename ... CallbackArgs>
+class BindContext : public InstanceContext
 {
 public:
   explicit BindContext(const std::string & name)
-  : InstanceContext<MessageT>(name, std::make_shared<BindContextImpl<MessageT, CallbackArgs ...>>())
+  : InstanceContext(name, std::make_shared<BindContextImpl<CallbackArgs ...>>())
   {}
 };
 
@@ -278,53 +232,13 @@ INSTANTIATE_TEST_SUITE_P(
       rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         const_ref_w_info_free_func)},
     // bind function
-    BindContext<test_msgs::msg::Empty, const test_msgs::msg::Empty &>("bind_method"),
-    BindContext<test_msgs::msg::Empty, const test_msgs::msg::Empty &, const rclcpp::MessageInfo &>(
+    BindContext<const test_msgs::msg::Empty &>("bind_method"),
+    BindContext<const test_msgs::msg::Empty &, const rclcpp::MessageInfo &>(
       "bind_method_with_info")
   ),
   format_parameter
 );
 
-void const_ref_ta_free_func(const MyEmpty &) {}
-void const_ref_ta_w_info_free_func(const MyEmpty &, const rclcpp::MessageInfo &) {}
-
-INSTANTIATE_TEST_SUITE_P(
-  ConstRefTACallbackTests,
-  DispatchTestsWithTA,
-  ::testing::Values(
-    // lambda
-    InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const MyEmpty &) {})},
-    InstanceContext<MyTA>{"lambda_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const MyEmpty &, const rclcpp::MessageInfo &) {})},
-    InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const test_msgs::msg::Empty &) {})},
-    InstanceContext<MyTA>{"lambda_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})},
-    // free function
-    InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_ta_free_func)},
-    InstanceContext<MyTA>{"free_function_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_ta_w_info_free_func)},
-    InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_free_func)},
-    InstanceContext<MyTA>{"free_function_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_w_info_free_func)},
-    // bind function
-    BindContext<MyTA, const MyEmpty &>("bind_method_ta"),
-    BindContext<MyTA, const MyEmpty &, const rclcpp::MessageInfo &>(
-      "bind_method_ta_with_info"),
-    BindContext<MyTA, const test_msgs::msg::Empty &>("bind_method"),
-    BindContext<MyTA, const test_msgs::msg::Empty &, const rclcpp::MessageInfo &>(
-      "bind_method_with_info")
-  ),
-  format_parameter_with_ta
-);
-
 //
 // Versions of `std::unique_ptr<MessageT, MessageDeleter>`
 //
@@ -350,54 +264,11 @@ INSTANTIATE_TEST_SUITE_P(
       rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         unique_ptr_w_info_free_func)},
     // bind function
-    BindContext<test_msgs::msg::Empty, std::unique_ptr<test_msgs::msg::Empty>>("bind_method"),
-    BindContext<
-      test_msgs::msg::Empty,
-      std::unique_ptr<test_msgs::msg::Empty>,
-      const rclcpp::MessageInfo &
-    >("bind_method_with_info")
-  ),
-  format_parameter
-);
-
-void unique_ptr_ta_free_func(std::unique_ptr<MyEmpty>) {}
-void unique_ptr_ta_w_info_free_func(std::unique_ptr<MyEmpty>, const rclcpp::MessageInfo &) {}
-
-INSTANTIATE_TEST_SUITE_P(
-  UniquePtrCallbackTests,
-  DispatchTestsWithTA,
-  ::testing::Values(
-    // lambda
-    InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::unique_ptr<MyEmpty>) {})},
-    InstanceContext<MyTA>{"lambda_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::unique_ptr<MyEmpty>, const rclcpp::MessageInfo &) {})},
-    InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::unique_ptr<test_msgs::msg::Empty>) {})},
-    InstanceContext<MyTA>{"lambda_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::unique_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {})},
-    // free function
-    InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        unique_ptr_ta_free_func)},
-    InstanceContext<MyTA>{"free_function_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        unique_ptr_ta_w_info_free_func)},
-    InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        unique_ptr_free_func)},
-    InstanceContext<MyTA>{"free_function_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        unique_ptr_w_info_free_func)},
-    // bind function
-    BindContext<MyTA, std::unique_ptr<test_msgs::msg::Empty>>("bind_method_ta"),
-    BindContext<MyTA, std::unique_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
-      "bind_method_ta_with_info"),
-    BindContext<MyTA, std::unique_ptr<test_msgs::msg::Empty>>("bind_method"),
-    BindContext<MyTA, std::unique_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
+    BindContext<std::unique_ptr<test_msgs::msg::Empty>>("bind_method"),
+    BindContext<std::unique_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
       "bind_method_with_info")
   ),
-  format_parameter_with_ta
+  format_parameter
 );
 
 //
@@ -425,56 +296,13 @@ INSTANTIATE_TEST_SUITE_P(
       rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         shared_const_ptr_w_info_free_func)},
     // bind function
-    BindContext<test_msgs::msg::Empty, std::shared_ptr<const test_msgs::msg::Empty>>("bind_method"),
-    BindContext<test_msgs::msg::Empty, std::shared_ptr<const test_msgs::msg::Empty>,
-    const rclcpp::MessageInfo &>(
+    BindContext<std::shared_ptr<const test_msgs::msg::Empty>>("bind_method"),
+    BindContext<std::shared_ptr<const test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
       "bind_method_with_info")
   ),
   format_parameter
 );
 
-void shared_const_ptr_ta_free_func(std::shared_ptr<const MyEmpty>) {}
-void shared_const_ptr_ta_w_info_free_func(
-  std::shared_ptr<const MyEmpty>, const rclcpp::MessageInfo &)
-{}
-
-INSTANTIATE_TEST_SUITE_P(
-  SharedConstPtrCallbackTests,
-  DispatchTestsWithTA,
-  ::testing::Values(
-    // lambda
-    InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<const MyEmpty>) {})},
-    InstanceContext<MyTA>{"lambda_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<const MyEmpty>, const rclcpp::MessageInfo &) {})},
-    InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<const test_msgs::msg::Empty>) {})},
-    InstanceContext<MyTA>{"lambda_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<const test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {})},
-    // free function
-    InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_const_ptr_ta_free_func)},
-    InstanceContext<MyTA>{"free_function_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_const_ptr_ta_w_info_free_func)},
-    InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_const_ptr_free_func)},
-    InstanceContext<MyTA>{"free_function_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_const_ptr_w_info_free_func)},
-    // bind function
-    BindContext<MyTA, std::shared_ptr<const MyEmpty>>("bind_method_ta"),
-    BindContext<MyTA, std::shared_ptr<const MyEmpty>, const rclcpp::MessageInfo &>(
-      "bind_method_ta_with_info"),
-    BindContext<MyTA, std::shared_ptr<const test_msgs::msg::Empty>>("bind_method"),
-    BindContext<MyTA, std::shared_ptr<const test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
-      "bind_method_with_info")
-  ),
-  format_parameter_with_ta
-);
-
 //
 // Versions of `const std::shared_ptr<const MessageT> &`
 //
@@ -500,58 +328,13 @@ INSTANTIATE_TEST_SUITE_P(
       rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         const_ref_shared_const_ptr_w_info_free_func)},
     // bind function
-    BindContext<test_msgs::msg::Empty,
-    const std::shared_ptr<const test_msgs::msg::Empty> &>("bind_method"),
-    BindContext<test_msgs::msg::Empty, const std::shared_ptr<const test_msgs::msg::Empty> &,
-    const rclcpp::MessageInfo &>(
+    BindContext<const std::shared_ptr<const test_msgs::msg::Empty> &>("bind_method"),
+    BindContext<const std::shared_ptr<const test_msgs::msg::Empty> &, const rclcpp::MessageInfo &>(
       "bind_method_with_info")
   ),
   format_parameter
 );
 
-void const_ref_shared_const_ptr_ta_free_func(const std::shared_ptr<const MyEmpty> &) {}
-void const_ref_shared_const_ptr_ta_w_info_free_func(
-  const std::shared_ptr<const MyEmpty> &, const rclcpp::MessageInfo &)
-{}
-
-INSTANTIATE_TEST_SUITE_P(
-  ConstRefSharedConstPtrCallbackTests,
-  DispatchTestsWithTA,
-  ::testing::Values(
-    // lambda
-    InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const std::shared_ptr<const MyEmpty> &) {})},
-    InstanceContext<MyTA>{"lambda_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const std::shared_ptr<const MyEmpty> &, const rclcpp::MessageInfo &) {})},
-    InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const std::shared_ptr<const test_msgs::msg::Empty> &) {})},
-    InstanceContext<MyTA>{"lambda_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](const std::shared_ptr<const test_msgs::msg::Empty> &, const rclcpp::MessageInfo &) {})},
-    // free function
-    InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_shared_const_ptr_ta_free_func)},
-    InstanceContext<MyTA>{"free_function_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_shared_const_ptr_ta_w_info_free_func)},
-    InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_shared_const_ptr_free_func)},
-    InstanceContext<MyTA>{"free_function_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        const_ref_shared_const_ptr_w_info_free_func)},
-    // bind function
-    BindContext<MyTA, const std::shared_ptr<const MyEmpty> &>("bind_method_ta"),
-    BindContext<MyTA, const std::shared_ptr<const MyEmpty> &, const rclcpp::MessageInfo &>(
-      "bind_method_ta_with_info"),
-    BindContext<MyTA, const std::shared_ptr<const test_msgs::msg::Empty> &>("bind_method"),
-    BindContext<MyTA, const std::shared_ptr<const test_msgs::msg::Empty> &,
-    const rclcpp::MessageInfo &>(
-      "bind_method_with_info")
-  ),
-  format_parameter_with_ta
-);
-
 //
 // Versions of `std::shared_ptr<MessageT>`
 //
@@ -577,52 +360,9 @@ INSTANTIATE_TEST_SUITE_P(
       rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty>().set(
         shared_ptr_w_info_free_func)},
     // bind function
-    BindContext<test_msgs::msg::Empty, std::shared_ptr<test_msgs::msg::Empty>>("bind_method"),
-    BindContext<test_msgs::msg::Empty, std::shared_ptr<test_msgs::msg::Empty>,
-    const rclcpp::MessageInfo &>(
+    BindContext<std::shared_ptr<test_msgs::msg::Empty>>("bind_method"),
+    BindContext<std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
       "bind_method_with_info")
   ),
   format_parameter
 );
-
-void shared_ptr_ta_free_func(std::shared_ptr<MyEmpty>) {}
-void shared_ptr_ta_w_info_free_func(
-  std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &)
-{}
-
-INSTANTIATE_TEST_SUITE_P(
-  SharedPtrCallbackTests,
-  DispatchTestsWithTA,
-  ::testing::Values(
-    // lambda
-    InstanceContext<MyTA>{"lambda_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<MyEmpty>) {})},
-    InstanceContext<MyTA>{"lambda_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &) {})},
-    InstanceContext<MyTA>{"lambda", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<test_msgs::msg::Empty>) {})},
-    InstanceContext<MyTA>{"lambda_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        [](std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &) {})},
-    // free function
-    InstanceContext<MyTA>{"free_function_ta", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_ptr_ta_free_func)},
-    InstanceContext<MyTA>{"free_function_ta_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_ptr_ta_w_info_free_func)},
-    InstanceContext<MyTA>{"free_function", rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_ptr_free_func)},
-    InstanceContext<MyTA>{"free_function_with_info",
-      rclcpp::AnySubscriptionCallback<MyTA>().set(
-        shared_ptr_w_info_free_func)},
-    // bind function
-    BindContext<MyTA, std::shared_ptr<MyEmpty>>("bind_method_ta"),
-    BindContext<MyTA, std::shared_ptr<MyEmpty>, const rclcpp::MessageInfo &>(
-      "bind_method_ta_with_info"),
-    BindContext<MyTA, std::shared_ptr<test_msgs::msg::Empty>>("bind_method"),
-    BindContext<MyTA, std::shared_ptr<test_msgs::msg::Empty>, const rclcpp::MessageInfo &>(
-      "bind_method_with_info")
-  ),
-  format_parameter_with_ta
-);