16.0.4

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,115 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.4 (2023-04-25)
+-------------------
+* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_) (`#2131 <https://github.com/ros2/rclcpp/issues/2131>`_)
+* Contributors: mergify[bot]
+
+16.0.3 (2023-01-10)
+-------------------
+* Fix SharedFuture from async_send_request never becomes valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_) (`#2076 <https://github.com/ros2/rclcpp/issues/2076>`_)
+* do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_) (`#2070 <https://github.com/ros2/rclcpp/issues/2070>`_)
+* fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_) (`#2065 <https://github.com/ros2/rclcpp/issues/2065>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_) (`#2033 <https://github.com/ros2/rclcpp/issues/2033>`_)
+* Contributors: mergify[bot]
+
+16.0.2 (2022-11-07)
+-------------------
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_) (`#2026 <https://github.com/ros2/rclcpp/issues/2026>`_)
+* use regex for wildcard matching (backport `#1839 <https://github.com/ros2/rclcpp/issues/1839>`_) (`#1986 <https://github.com/ros2/rclcpp/issues/1986>`_)
+* Drop wrong template specialization (`#1926 <https://github.com/ros2/rclcpp/issues/1926>`_) (`#1937 <https://github.com/ros2/rclcpp/issues/1937>`_)
+* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1932 <https://github.com/ros2/rclcpp/issues/1932>`_)
+* Contributors: mergify[bot]
+
+16.0.1 (2022-04-13)
+-------------------
+* remove DEFINE_CONTENT_FILTER cmake option (`#1914 <https://github.com/ros2/rclcpp/issues/1914>`_)
+* Contributors: Chen Lihui
+
+16.0.0 (2022-04-08)
+-------------------
+* remove things that were deprecated during galactic (`#1913 <https://github.com/ros2/rclcpp/issues/1913>`_)
+* Contributors: William Woodall
+
+15.4.0 (2022-04-05)
+-------------------
+* add take_data_by_entity_id API to waitable (`#1892 <https://github.com/ros2/rclcpp/issues/1892>`_)
+* add content-filtered-topic interfaces (`#1561 <https://github.com/ros2/rclcpp/issues/1561>`_)
+* Contributors: Alberto Soragna, Chen Lihui
+
+15.3.0 (2022-03-30)
+-------------------
+* [NodeParameters] Set name in param info pre-check (`#1908 <https://github.com/ros2/rclcpp/issues/1908>`_)
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_)
+* Add publish by loaned message in GenericPublisher (`#1856 <https://github.com/ros2/rclcpp/issues/1856>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard
+
+15.2.0 (2022-03-24)
+-------------------
+* Add missing ament dependency on rcl_interfaces (`#1903 <https://github.com/ros2/rclcpp/issues/1903>`_)
+* Update data callback tests to account for all published samples (`#1900 <https://github.com/ros2/rclcpp/issues/1900>`_)
+* Increase timeout for acknowledgments to account for slower Connext settings (`#1901 <https://github.com/ros2/rclcpp/issues/1901>`_)
+* clang-tidy: explicit constructors (`#1782 <https://github.com/ros2/rclcpp/issues/1782>`_)
+* Add client/service QoS getters (`#1784 <https://github.com/ros2/rclcpp/issues/1784>`_)
+* Fix a bunch more rosdoc2 issues in rclcpp. (`#1897 <https://github.com/ros2/rclcpp/issues/1897>`_)
+* time_until_trigger returns max time if timer is cancelled (`#1893 <https://github.com/ros2/rclcpp/issues/1893>`_)
+* Micro-optimizations in rclcpp (`#1896 <https://github.com/ros2/rclcpp/issues/1896>`_)
+* Contributors: Andrea Sorbini, Chris Lalancette, Mauro Passerino, Scott K Logan, William Woodall
+
+15.1.0 (2022-03-01)
+-------------------
+* spin_all with a zero timeout. (`#1878 <https://github.com/ros2/rclcpp/issues/1878>`_)
+* Add RMW listener APIs (`#1579 <https://github.com/ros2/rclcpp/issues/1579>`_)
+* Remove fastrtps customization on tests (`#1887 <https://github.com/ros2/rclcpp/issues/1887>`_)
+* Install headers to include/${PROJECT_NAME} (`#1888 <https://github.com/ros2/rclcpp/issues/1888>`_)
+* Use ament_generate_version_header (`#1886 <https://github.com/ros2/rclcpp/issues/1886>`_)
+* use universal reference to support rvalue. (`#1883 <https://github.com/ros2/rclcpp/issues/1883>`_)
+* fix one subscription can wait_for_message twice (`#1870 <https://github.com/ros2/rclcpp/issues/1870>`_)
+* Add return value version of get_parameter_or (`#1813 <https://github.com/ros2/rclcpp/issues/1813>`_)
+* Cleanup time source object lifetimes (`#1867 <https://github.com/ros2/rclcpp/issues/1867>`_)
+* add is_spinning() method to executor base class
+* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Kenji Miyake, Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS
+
+15.0.0 (2022-01-14)
+-------------------
+* Cleanup the TypeAdapt tests (`#1858 <https://github.com/ros2/rclcpp/issues/1858>`_)
+* Cleanup includes (`#1857 <https://github.com/ros2/rclcpp/issues/1857>`_)
+* Fix include order and relative paths for cpplint (`#1859 <https://github.com/ros2/rclcpp/issues/1859>`_)
+* Rename stringstream in macros to a more unique name (`#1862 <https://github.com/ros2/rclcpp/issues/1862>`_)
+* Add non transform capabilities for intra-process (`#1849 <https://github.com/ros2/rclcpp/issues/1849>`_)
+* Fix rclcpp documentation build (`#1779 <https://github.com/ros2/rclcpp/issues/1779>`_)
+* Contributors: Chris Lalancette, Doug Smith, Gonzo, Jacob Perron, Michel Hidalgo
+
+14.1.0 (2022-01-05)
+-------------------
+* Use UninitializedStaticallyTypedParameterException (`#1689 <https://github.com/ros2/rclcpp/issues/1689>`_)
+* Add wait_for_all_acked support (`#1662 <https://github.com/ros2/rclcpp/issues/1662>`_)
+* Add tests for function templates of declare_parameter (`#1747 <https://github.com/ros2/rclcpp/issues/1747>`_)
+* Contributors: Barry Xu, Bi0T1N, M. Mostafa Farzan
+
+14.0.0 (2021-12-17)
+-------------------
+* Fixes for uncrustify 0.72 (`#1844 <https://github.com/ros2/rclcpp/issues/1844>`_)
+* use private member to keep the all reference underneath. (`#1845 <https://github.com/ros2/rclcpp/issues/1845>`_)
+* Make node base sharable (`#1832 <https://github.com/ros2/rclcpp/issues/1832>`_)
+* Add Clock::sleep_for() (`#1828 <https://github.com/ros2/rclcpp/issues/1828>`_)
+* Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (`#1830 <https://github.com/ros2/rclcpp/issues/1830>`_)
+* Use rclcpp::guard_condition (`#1612 <https://github.com/ros2/rclcpp/issues/1612>`_)
+* Call CMake function to generate version header (`#1805 <https://github.com/ros2/rclcpp/issues/1805>`_)
+* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_)
+* Remove author by request (`#1818 <https://github.com/ros2/rclcpp/issues/1818>`_)
+* Update maintainers (`#1817 <https://github.com/ros2/rclcpp/issues/1817>`_)
+* min_forward & min_backward thresholds must not be disabled (`#1815 <https://github.com/ros2/rclcpp/issues/1815>`_)
+* Re-add Clock::sleep_until (`#1814 <https://github.com/ros2/rclcpp/issues/1814>`_)
+* Fix lifetime of context so it remains alive while its dependent node handles are still in use (`#1754 <https://github.com/ros2/rclcpp/issues/1754>`_)
+* Add the interface for pre-shutdown callback (`#1714 <https://github.com/ros2/rclcpp/issues/1714>`_)
+* Take message ownership from moved LoanedMessage (`#1808 <https://github.com/ros2/rclcpp/issues/1808>`_)
+* Suppress clang dead-store warnings in the benchmarks. (`#1802 <https://github.com/ros2/rclcpp/issues/1802>`_)
+* Wait for publisher and subscription to match (`#1777 <https://github.com/ros2/rclcpp/issues/1777>`_)
+* Fix unused QoS profile for clock subscription and make ClockQoS the default (`#1801 <https://github.com/ros2/rclcpp/issues/1801>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Chen Lihui, Chris Lalancette, Grey, Jacob Perron, Nikolai Morin, Shane Loretz, Tomoya Fujita, mauropasse
+
 13.1.0 (2021-10-18)
 -------------------
 * Fix dangerous std::bind capture in TimeSource implementation. (`#1768 <https://github.com/ros2/rclcpp/issues/1768>`_)

rclcpp/CMakeLists.txt

@@ -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/add_guard_condition_to_rcl_wait_set.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
@@ -186,13 +187,14 @@ endif()
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
-  "$<INSTALL_INTERFACE:include>")
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
   "ament_index_cpp"
   "libstatistics_collector"
   "rcl"
+  "rcl_interfaces"
   "rcl_yaml_param_parser"
   "rcpputils"
   "rcutils"
@@ -216,11 +218,14 @@ install(
   RUNTIME DESTINATION bin
 )
 
-# specific order: dependents before dependencies
-ament_export_include_directories(include)
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(${PROJECT_NAME})
+
+# Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
+# specific order: dependents before dependencies
 ament_export_dependencies(ament_index_cpp)
 ament_export_dependencies(libstatistics_collector)
 ament_export_dependencies(rcl)
@@ -246,7 +251,7 @@ ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(TEST cppcheck)
@@ -254,4 +259,4 @@ if(TEST cppcheck)
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
 
-ament_cmake_gen_version_h()
+ament_generate_version_header(${PROJECT_NAME})

rclcpp/Doxyfile

@@ -21,13 +21,22 @@ GENERATE_LATEX         = NO
 ENABLE_PREPROCESSING   = YES
 MACRO_EXPANSION        = YES
 EXPAND_ONLY_PREDEF     = YES
-PREDEFINED             = RCLCPP_PUBLIC=
+PREDEFINED             += DOXYGEN_ONLY
+PREDEFINED             += RCLCPP_LOCAL=
+PREDEFINED             += RCLCPP_PUBLIC=
+PREDEFINED             += RCLCPP_PUBLIC_TYPE=
+PREDEFINED             += RCUTILS_WARN_UNUSED=
+PREDEFINED             += RCPPUTILS_TSA_GUARDED_BY(x)=
+PREDEFINED             += RCPPUTILS_TSA_PT_GUARDED_BY(x)=
+PREDEFINED             += RCPPUTILS_TSA_REQUIRES(x)=
+
+DOT_GRAPH_MAX_NODES    = 101
 
 # Tag files that do not exist will produce a warning and cross-project linking will not work.
-TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
+#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
 # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0)
-TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
-TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
-TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
+#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
+#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
+#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
 # Uncomment to generate tag files for cross-project linking.
 #GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag"

rclcpp/include/rclcpp/allocator/allocator_common.hpp

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

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -41,7 +41,7 @@ public:
   }
 
   template<typename T>
-  AllocatorDeleter(const AllocatorDeleter<T> & a)
+  explicit AllocatorDeleter(const AllocatorDeleter<T> & a)
   {
     allocator_ = a.get_allocator();
   }

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -354,12 +354,6 @@ public:
     allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
   }
 
-  [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]]
-  explicit
-  AnySubscriptionCallback(std::shared_ptr<AllocatorT> allocator)  // NOLINT[runtime/explicit]
-  : AnySubscriptionCallback(*NotNull<AllocatorT>(allocator.get(), "invalid allocator").pointer)
-  {}
-
   AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
 
   /// Generic function for setting the callback.
@@ -477,7 +471,7 @@ public:
   }
 
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
-  convert_custom_type_message_to_ros_message_unique_ptr(const SubscribedType & msg)
+  convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg)
   {
     if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
       auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
@@ -485,13 +479,14 @@ public:
       rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ptr);
       return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
     } else {
-      throw std::runtime_error(
-              "convert_custom_type_message_to_ros_message_unique_ptr "
-              "unexpectedly called without TypeAdapter");
+      static_assert(
+        !sizeof(MessageT *),
+        "convert_custom_type_to_ros_message_unique_ptr() "
+        "unexpectedly called without specialized TypeAdapter");
     }
   }
 
-  // Dispatch when input is a ROS message and the output could be anything.
+  // Dispatch when input is a ros message and the output could be anything.
   void
   dispatch(
     std::shared_ptr<ROSMessageType> message,
@@ -544,7 +539,7 @@ public:
         {
           callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
         }
-        // conditions for output is ROS message
+        // conditions for output is ros message
         else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
           callback(*message);
         } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
@@ -671,14 +666,9 @@ public:
     TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
-  template<typename T>
-  typename
-  std::enable_if_t<
-    rosidl_generator_traits::is_message<T>::value &&
-    std::is_same<T, ROSMessageType>::value
-  >
+  void
   dispatch_intra_process(
-    std::shared_ptr<const ROSMessageType> message,
+    std::shared_ptr<const SubscribedType> message,
     const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), true);
@@ -692,304 +682,107 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && callback) {
-        using CallbackT = std::decay_t<decltype(callback)>;
+        using T = std::decay_t<decltype(callback)>;
         static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
 
         // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
-        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, UniquePtrCallback>||
-            std::is_same_v<CallbackT, SharedPtrCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
-            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
-        }
-        // conditions for ROS message type
-        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
+        if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
           callback(*message);
-        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
+        } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
           callback(*message, message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
-        {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
-        {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
-        {
-          callback(message);
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
-        {
-          callback(message, message_info);
-        }
-        // condition to catch SerializedMessage types
-        else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
-        {
-          throw std::runtime_error(
-            "Cannot dispatch std::shared_ptr<const ROSMessageType> message "
-            "to rclcpp::SerializedMessage");
-        }
-        // condition to catch unhandled callback types
-        else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<CallbackT>, "unhandled callback type");
-        }
-      }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
-  }
-
-  template<typename T>
-  typename
-  std::enable_if_t<
-    rosidl_generator_traits::is_message<T>::value &&
-    std::is_same<T, ROSMessageType>::value
-  >
-  dispatch_intra_process(
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message,
-    const rclcpp::MessageInfo & message_info)
-  {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
-    // Check if the variant is "unset", throw if it is.
-    if (callback_variant_.index() == 0) {
-      if (std::get<0>(callback_variant_) == nullptr) {
-        // This can happen if it is default initialized, or if it is assigned nullptr.
-        throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback");
-      }
-    }
-    // Dispatch.
-    std::visit(
-      [&message, &message_info, this](auto && callback) {
-        using CallbackT = std::decay_t<decltype(callback)>;
-        static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
-
-        // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
-        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, UniquePtrCallback>||
-            std::is_same_v<CallbackT, SharedPtrCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
-            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
-        ))
-        {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
-        }
-        // conditions for ROS message type
-        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
-          callback(*message, message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
-        {
-          callback(std::move(message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
-        {
-          callback(std::move(message), message_info);
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
-        {
-          callback(std::move(message));
-        } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
-        {
-          callback(std::move(message), message_info);
-        }
-        // condition to catch SerializedMessage types
-        else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
-        {
-          throw std::runtime_error(
-            "Cannot dispatch std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message "
-            "to rclcpp::SerializedMessage");
-        }
-        // condition to catch unhandled callback types
-        else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<CallbackT>, "unhandled callback type");
-        }
-      }, callback_variant_);
-    TRACEPOINT(callback_end, static_cast<const void *>(this));
-  }
-
-  template<typename T>
-  typename std::enable_if_t<
-    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
-    std::is_same<T, SubscribedType>::value
-  >
-  dispatch_intra_process(
-    std::shared_ptr<const T> message,
-    const rclcpp::MessageInfo & message_info)
-  {
-    TRACEPOINT(callback_start, static_cast<const void *>(this), true);
-    // Check if the variant is "unset", throw if it is.
-    if (callback_variant_.index() == 0) {
-      if (std::get<0>(callback_variant_) == nullptr) {
-        // This can happen if it is default initialized, or if it is assigned nullptr.
-        throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback");
-      }
-    }
-    // Dispatch.
-    std::visit(
-      [&message, &message_info, this](auto && callback) {
-        using CallbackT = std::decay_t<decltype(callback)>;
-        static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
-
-        // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
-          callback(*message);
-        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (  // NOLINT[readability/braces]
-          is_ta && (
-            std::is_same_v<CallbackT, UniquePtrCallback>||
-            std::is_same_v<CallbackT, SharedPtrCallback>
+            std::is_same_v<T, UniquePtrCallback>||
+            std::is_same_v<T, SharedPtrCallback>
         ))
         {
           callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
-            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
+            std::is_same_v<T, UniquePtrWithInfoCallback>||
+            std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
           callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
+            std::is_same_v<T, SharedConstPtrCallback>||
+            std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
-          callback(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(message.get(), subscribed_type_deleter_));
+          callback(message);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
+            std::is_same_v<T, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
-          callback(std::unique_ptr<SubscribedType, SubscribedTypeDeleter>(message.get(), subscribed_type_deleter_), message_info);
+          callback(message, message_info);
         }
-        // conditions for ROS message type
-        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
-          auto local_message = convert_custom_type_message_to_ros_message_unique_ptr(*message);
-          callback(*local_message);
-        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
-          auto local_message = convert_custom_type_message_to_ros_message_unique_ptr(*message);
-          callback(*local_message, message_info);
+        // conditions for ros message type
+        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local);
+          } else {
+            callback(*message);
+          }
+        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local, message_info);
+          } else {
+            callback(*message, message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
+          std::is_same_v<T, UniquePtrROSMessageCallback>||
+          std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
+          std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
+          std::is_same_v<T, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(message);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+          std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(convert_custom_type_message_to_ros_message_unique_ptr(*message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(message, message_info);
+          }
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
+          std::is_same_v<T, ConstRefSerializedMessageCallback>||
+          std::is_same_v<T, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<T, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<T, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<T, SharedPtrSerializedMessageWithInfoCallback>)
         {
           throw std::runtime_error(
             "Cannot dispatch std::shared_ptr<const ROSMessageType> message "
@@ -997,19 +790,15 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<CallbackT>, "unhandled callback type");
+          static_assert(always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
     TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
-  template<typename T>
-  typename std::enable_if_t<
-    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
-    std::is_same<T, SubscribedType>::value
-  >
+  void
   dispatch_intra_process(
-    std::unique_ptr<T, SubscribedTypeDeleter> message,
+    std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
     const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), true);
@@ -1023,83 +812,111 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && callback) {
-        using CallbackT = std::decay_t<decltype(callback)>;
+        // clang complains that 'this' lambda capture is unused, which is true
+        // in *some* specializations of this template, but not others.  Just
+        // quiet it down.
+        (void)this;
+
+        using T = std::decay_t<decltype(callback)>;
         static constexpr bool is_ta = rclcpp::TypeAdapter<MessageT>::is_specialized::value;
 
         // conditions for custom type
-        if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefCallback>) {
-          callback(*message);
-        } else if constexpr (is_ta && std::is_same_v<CallbackT, ConstRefWithInfoCallback>) {  // NOLINT
+        if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
           callback(*message);
+        } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
+          callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, UniquePtrCallback>||
-            std::is_same_v<CallbackT, SharedPtrCallback>
-        ))
+            std::is_same_v<T, UniquePtrCallback>||
+            std::is_same_v<T, SharedPtrCallback>))
         {
           callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, UniquePtrWithInfoCallback>||
-            std::is_same_v<CallbackT, SharedPtrWithInfoCallback>
+            std::is_same_v<T, UniquePtrWithInfoCallback>||
+            std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
           callback(std::move(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrCallback>
+            std::is_same_v<T, SharedConstPtrCallback>||
+            std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
           callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
-            std::is_same_v<CallbackT, SharedConstPtrWithInfoCallback>||
-            std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoCallback>
+            std::is_same_v<T, SharedConstPtrWithInfoCallback>||
+            std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
           callback(std::move(message), message_info);
         }
-        // conditions for ROS message type
-        else if constexpr (std::is_same_v<CallbackT, ConstRefROSMessageCallback>) {  // NOLINT
-          auto local_message = convert_custom_type_to_ros_message_unique_ptr(*message);
-          callback(*local_message);
-        } else if constexpr (std::is_same_v<CallbackT, ConstRefWithInfoROSMessageCallback>) {
-          auto local_message = convert_custom_type_to_ros_message_unique_ptr(*message);
-          callback(*local_message, message_info);
+        // conditions for ros message type
+        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local);
+          } else {
+            callback(*message);
+          }
+        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {  // NOLINT[readability/braces]
+          if constexpr (is_ta) {
+            auto local = convert_custom_type_to_ros_message_unique_ptr(*message);
+            callback(*local, message_info);
+          } else {
+            callback(*message, message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrROSMessageCallback>)
+          std::is_same_v<T, UniquePtrROSMessageCallback>||
+          std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(std::move(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, UniquePtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrWithInfoROSMessageCallback>)
+          std::is_same_v<T, UniquePtrWithInfoROSMessageCallback>||
+          std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(std::move(message), message_info);
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrROSMessageCallback>)
+          std::is_same_v<T, SharedConstPtrROSMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message));
+          } else {
+            callback(std::move(message));
+          }
         } else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, SharedConstPtrWithInfoROSMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
+          std::is_same_v<T, SharedConstPtrWithInfoROSMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          if constexpr (is_ta) {
+            callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info);
+          } else {
+            callback(std::move(message), message_info);
+          }
         }
         // condition to catch SerializedMessage types
         else if constexpr (  // NOLINT[readability/braces]
-          std::is_same_v<CallbackT, ConstRefSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, UniquePtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageCallback>||
-          std::is_same_v<CallbackT, SharedPtrSerializedMessageWithInfoCallback>)
+          std::is_same_v<T, ConstRefSerializedMessageCallback>||
+          std::is_same_v<T, ConstRefSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, UniquePtrSerializedMessageCallback>||
+          std::is_same_v<T, UniquePtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, SharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<T, SharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageCallback>||
+          std::is_same_v<T, ConstRefSharedConstPtrSerializedMessageWithInfoCallback>||
+          std::is_same_v<T, SharedPtrSerializedMessageCallback>||
+          std::is_same_v<T, SharedPtrSerializedMessageWithInfoCallback>)
         {
           throw std::runtime_error(
             "Cannot dispatch std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message "
@@ -1107,7 +924,7 @@ public:
         }
         // condition to catch unhandled callback types
         else {  // NOLINT[readability/braces]
-          static_assert(always_false_v<CallbackT>, "unhandled callback type");
+          static_assert(always_false_v<T>, "unhandled callback type");
         }
       }, callback_variant_);
     TRACEPOINT(callback_end, static_cast<const void *>(this));

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -95,6 +98,10 @@ public:
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  ~CallbackGroup();
+
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
   find_subscription_ptrs_if(Function func) const
@@ -138,6 +145,14 @@ public:
   const CallbackGroupType &
   type() const;
 
+  RCLCPP_PUBLIC
+  void collect_all_ptrs(
+    std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+    std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+    std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+    std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+    std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const;
+
   /// Return a reference to the 'associated with executor' atomic boolean.
   /**
    * When a callback group is added to an executor this boolean is checked
@@ -163,6 +178,16 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
+  /// Defer creating the notify guard condition and return it.
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -205,6 +230,9 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -19,6 +19,7 @@
 #include <future>
 #include <unordered_map>
 #include <memory>
+#include <mutex>
 #include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
@@ -29,20 +30,23 @@
 
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcutils/logging_macros.h"
-
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
 
 namespace rclcpp
@@ -215,6 +219,122 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual request publsher 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 client, 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.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request publsher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_publisher_actual_qos() const;
+
+  /// Get the actual response subscription 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 client, 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.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new response is received.
+  /**
+   * The callback receives a size_t which is the number of responses received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if responses were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the client
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_client_set_on_new_response_callback
+   * \sa rcl_client_set_on_new_response_callback
+   *
+   * \param[in] callback functor to be called when a new response is received
+   */
+  void
+  set_on_new_response_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_response_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_responses) {
+        try {
+          callback(number_of_responses);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new response' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new response' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_response_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_response_callback_));
+  }
+
+  /// Unset the callback registered for new responses, if any.
+  void
+  clear_on_new_response_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_response_callback_) {
+      set_on_new_response_callback(nullptr, nullptr);
+      on_new_response_callback_ = nullptr;
+    }
+  }
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 
@@ -230,13 +350,21 @@ protected:
   const rcl_node_t *
   get_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data);
+
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rclcpp::Context> context_;
+  rclcpp::Logger node_logger_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
 
 template<typename ServiceT>
@@ -641,7 +769,9 @@ public:
     auto old_size = pending_requests_.size();
     for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
       if (it->second.first < time_point) {
-        pruned_requests->push_back(it->first);
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
         it = pending_requests_.erase(it);
       } else {
         ++it;
@@ -664,16 +794,14 @@ protected:
   async_send_request_impl(const Request & request, CallbackInfoVariant value)
   {
     int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
-    {
-      std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-      pending_requests_.try_emplace(
-        sequence_number,
-        std::make_pair(std::chrono::system_clock::now(), std::move(value)));
-    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
     return sequence_number;
   }
 

rclcpp/include/rclcpp/clock.hpp

@@ -88,6 +88,16 @@ public:
    *     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.
    *
+   * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function
+   *    `pthread_cond_clockwait`, steady clocks may sleep using the system clock.
+   *    If so, steady clock sleep times can be affected by system clock time jumps.
+   *    Depending on the steady clock's epoch and resolution in comparison to the system clock's,
+   *    an overflow when converting steady clock durations to system clock times may cause
+   *    undefined behavior.
+   *    For more info see these issues:
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931
+   *
    * \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
@@ -103,6 +113,30 @@ public:
     Time until,
     Context::SharedPtr context = contexts::get_global_default_context());
 
+  /**
+   * Sleep for a specified Duration.
+   *
+   * Equivalent to
+   *
+   * ```cpp
+   * clock->sleep_until(clock->now() + rel_time, context)
+   * ```
+   *
+   * The function will return immediately if `rel_time` is zero or negative.
+   *
+   * \param rel_time the length of time to sleep for.
+   * \param context the rclcpp context the clock should use to check that ROS is still initialized.
+   * \return true when the end time 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
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(
+    Duration rel_time,
+    Context::SharedPtr context = contexts::get_global_default_context());
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/context.hpp

@@ -127,7 +127,7 @@ public:
   void
   init(
     int argc,
-    char const * const argv[],
+    char const * const * argv,
     const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
 
   /// Return true if the context is valid, otherwise false.

rclcpp/include/rclcpp/create_publisher.hpp

@@ -92,7 +92,7 @@ template<
   typename NodeT>
 std::shared_ptr<PublisherT>
 create_publisher(
-  NodeT & node,
+  NodeT && node,
   const std::string & topic_name,
   const rclcpp::QoS & qos,
   const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (

rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp

@@ -0,0 +1,39 @@
+// 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__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_
+
+#include "rclcpp/guard_condition.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Adds the guard condition to a waitset
+/**
+ * \param[in] wait_set reference to a wait set where to add the guard condition
+ * \param[in] guard_condition reference to the guard_condition to be added
+ */
+RCLCPP_PUBLIC
+void
+add_guard_condition_to_rcl_wait_set(
+  rcl_wait_set_t & wait_set,
+  const rclcpp::GuardCondition & guard_condition);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -0,0 +1,68 @@
+// 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__CPP_CALLBACK_TRAMPOLINE_HPP_
+#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_
+
+#include <functional>
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Trampoline pattern for wrapping std::function into C-style callbacks.
+/**
+ * A common pattern in C is for a function to take a function pointer and a
+ * void pointer for "user data" which is passed to the function pointer when it
+ * is called from within C.
+ *
+ * It works by using the user data pointer to store a pointer to a
+ * std::function instance.
+ * So when called from C, this function will cast the user data to the right
+ * std::function type and call it.
+ *
+ * This should allow you to use free functions, lambdas with and without
+ * captures, and various kinds of std::bind instances.
+ *
+ * The interior of this function is likely to be executed within a C runtime,
+ * so no exceptions should be thrown at this point, and doing so results in
+ * undefined behavior.
+ *
+ * \tparam UserDataT Deduced type based on what is passed for user data,
+ *   usually this type is either `void *` or `const void *`.
+ * \tparam Args the arguments being passed to the callback
+ * \tparam ReturnT the return type of this function and the callback, default void
+ * \param user_data the function pointer, possibly type erased
+ * \param args the arguments to be forwarded to the callback
+ * \returns whatever the callback returns, if anything
+ */
+template<
+  typename UserDataT,
+  typename ... Args,
+  typename ReturnT = void
+>
+ReturnT
+cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
+{
+  auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
+  return actual_callback(args ...);
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -104,6 +104,7 @@ declare_parameter_or_get(
   }
 }
 
+#ifdef DOXYGEN_ONLY
 /// \internal Declare QoS parameters for the given entity.
 /**
  * \tparam NodeT Node pointer or reference type.
@@ -116,12 +117,23 @@ declare_parameter_or_get(
  * \param default_qos User provided qos. It will be used as a default for the parameters declared.
  * \return qos profile based on the user provided parameter overrides.
  */
+template<typename NodeT, typename EntityQosParametersTraits>
+rclcpp::QoS
+  declare_qos_parameters(
+  const ::rclcpp::QosOverridingOptions & options,
+  NodeT & node,
+  const std::string & topic_name,
+  const ::rclcpp::QoS & default_qos,
+  EntityQosParametersTraits);
+
+#else
+
 template<typename NodeT, typename EntityQosParametersTraits>
 std::enable_if_t<
-  rclcpp::node_interfaces::has_node_parameters_interface<
+  (rclcpp::node_interfaces::has_node_parameters_interface<
     decltype(std::declval<typename rcpputils::remove_pointer<NodeT>::type>())>::value ||
   std::is_same<typename std::decay_t<NodeT>,
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value),
   rclcpp::QoS>
 declare_qos_parameters(
   const ::rclcpp::QosOverridingOptions & options,
@@ -204,6 +216,8 @@ declare_qos_parameters(
   return default_qos;
 }
 
+#endif
+
 /// \internal Helper function to get a rmw qos policy value from a string.
 #define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \
     kind_lower, kind_upper, parameter_value, rclcpp_qos) \

rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp

@@ -38,6 +38,7 @@ namespace detail
  *
  * This issue, with the lambda's, can be demonstrated with this minimal program:
  *
+ * \code{.cpp}
  *   #include <functional>
  *   #include <memory>
  *
@@ -52,6 +53,7 @@ namespace detail
  *     std::function<void (std::shared_ptr<int>)> cb = [](std::shared_ptr<int>){};
  *     f(cb);
  *   }
+ * \endcode
  *
  * If this program ever starts working in a future version of C++, this class
  * may become redundant.

rclcpp/include/rclcpp/detail/utilities.hpp

@@ -30,7 +30,7 @@ namespace detail
 
 std::vector<std::string>
 get_unparsed_ros_arguments(
-  int argc, char const * const argv[],
+  int argc, char const * const * argv,
   rcl_arguments_t * arguments,
   rcl_allocator_t allocator);
 

rclcpp/include/rclcpp/duration.hpp

@@ -38,13 +38,6 @@ public:
    */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  /// Construct duration from the specified nanoseconds.
-  [[deprecated(
-    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
-    "rclcpp::Duration::from_nanoseconds(int64_variable);"
-    "rclcpp::Duration(0ns);")]]
-  explicit Duration(rcl_duration_value_t nanoseconds);
-
   /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -109,6 +109,8 @@ public:
   : std::runtime_error(msg) {}
 };
 
+typedef void (* reset_error_function_t)();
+
 /// Throw a C++ std::exception which was created based on an rcl error.
 /**
  * Passing nullptr for reset_error is safe and will avoid calling any function
@@ -129,7 +131,7 @@ throw_from_rcl_error [[noreturn]] (
   rcl_ret_t ret,
   const std::string & prefix = "",
   const rcl_error_state_t * error_state = nullptr,
-  void (* reset_error)() = rcl_reset_error);
+  reset_error_function_t reset_error = rcl_reset_error);
 /* *INDENT-ON* */
 
 class RCLErrorBase
@@ -254,6 +256,23 @@ public:
   {}
 };
 
+/// Thrown if user attempts to create an uninitialized statically typed parameter
+/**
+ * (see https://github.com/ros2/rclcpp/issues/1691)
+ */
+class UninitializedStaticallyTypedParameterException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   */
+  RCLCPP_PUBLIC
+  explicit UninitializedStaticallyTypedParameterException(const std::string & name)
+  : std::runtime_error("Statically typed parameter '" + name + "' must be initialized.")
+  {}
+};
+
 /// Thrown if parameter is already declared.
 class ParameterAlreadyDeclaredException : public std::runtime_error
 {
@@ -289,7 +308,6 @@ public:
   /// Construct an instance.
   /**
    * \param[in] name the name of the parameter.
-   * \param[in] message custom exception message.
    */
   explicit ParameterUninitializedException(const std::string & name)
   : std::runtime_error("parameter '" + name + "' is not initialized")

rclcpp/include/rclcpp/executor.hpp

@@ -305,7 +305,9 @@ public:
    * If the time that waitables take to be executed is longer than the period on which new waitables
    * become ready, this method will execute work repeatedly until `max_duration` has elapsed.
    *
-   * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive.
+   * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0.
+   *   `0` is potentially block forever until no more work is available.
+   * \throw std::invalid_argument if max_duration is less than 0.
    * Note that spin_all() may take longer than this time as it only returns once max_duration has
    * been exceeded.
    */
@@ -401,6 +403,15 @@ public:
   void
   set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
+  /// Returns true if the executor is currently spinning.
+  /**
+   * This function can be called asynchronously from any thread.
+   * \return True if the executor is currently spinning.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_spinning();
+
 protected:
   RCLCPP_PUBLIC
   void
@@ -455,6 +466,7 @@ protected:
   /// Return true if the node has been added to this executor.
   /**
    * \param[in] node_ptr a shared pointer that points to a node base interface
+   * \param[in] weak_groups_to_nodes map to nodes to lookup
    * \return true if the node is associated with the executor, otherwise false
    */
   RCLCPP_PUBLIC
@@ -525,7 +537,7 @@ protected:
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition interrupt_guard_condition_;
 
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
@@ -548,14 +560,14 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -52,7 +52,7 @@ public:
    * \param timeout maximum time to wait
    */
   RCLCPP_PUBLIC
-  MultiThreadedExecutor(
+  explicit MultiThreadedExecutor(
     const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false,

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -102,7 +102,7 @@ public:
    * \throws std::runtime_error if it couldn't add guard condition to wait set
    */
   RCLCPP_PUBLIC
-  bool
+  void
   add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
   RCLCPP_PUBLIC
@@ -299,7 +299,8 @@ private:
 
   /// Return true if the node belongs to the collector
   /**
-   * \param[in] group_ptr a node base interface shared pointer
+   * \param[in] node_ptr a node base interface shared pointer
+   * \param[in] weak_groups_to_nodes map to nodes to lookup
    * \return boolean whether a node belongs the collector
    */
   bool
@@ -328,7 +329,7 @@ private:
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
 
   typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
+      const rclcpp::GuardCondition *,
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
   WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -99,9 +99,10 @@ public:
 
   /// Static executor implementation of spin all
   /**
-   * This non-blocking function will execute entities until
-   * timeout or no more work available. If new entities get ready
-   * while executing work available, they will be executed
+   * This non-blocking function will execute entities until timeout (must be >= 0)
+   * or no more work available.
+   * If timeout is `0`, potentially it blocks forever until no more work is available.
+   * If new entities get ready while executing work available, they will be executed
    * as long as the timeout hasn't expired.
    *
    * Example:

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

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -22,14 +22,13 @@
 #include <iterator>
 #include <memory>
 #include <stdexcept>
-#include <type_traits>
 #include <unordered_map>
 #include <utility>
 #include <vector>
-
-#include "rosidl_runtime_cpp/traits.hpp"
+#include <typeinfo>
 
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
@@ -173,34 +172,22 @@ public:
    *
    * \param intra_process_publisher_id the id of the publisher of this message.
    * \param message the message that is being stored.
+   * \param allocator for allocations when buffering messages.
    */
   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 ROSMessageType,
+    typename Alloc,
+    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,
+    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 +203,54 @@ 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, ROSMessageType>(
         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<MessageT, Alloc, Deleter, ROSMessageType>(
+        std::move(message),
+        concatenated_vector,
+        allocator);
+    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
+      sub_ids.take_shared_subscriptions.size() > 1)
+    {
+      // Construct a new shared pointer from the message
+      // for the buffers that do not require ownership
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
-        this->template add_owned_msg_to_buffers<ROSMessageType, AllocatorT>(
-          std::move(message),
-          concatenated_vector,
-          ros_message_type_allocator);
-      } else {
-        // Construct a new shared pointer from the message
-        // for the buffers that do not require ownership
-        auto shared_msg = std::allocate_shared<ROSMessageType, ROSMessageTypeAllocator>(
-          ros_message_type_allocator, *message);
-
-        this->template add_shared_msg_to_buffers<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, ROSMessageType>(
+        shared_msg, sub_ids.take_shared_subscriptions);
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
+        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 ROSMessageType,
+    typename Alloc,
+    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,
+    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 +266,32 @@ 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, ROSMessageType>(
           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, ROSMessageType>(
           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, ROSMessageType>(
+          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
@@ -529,69 +339,22 @@ private:
 
   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,
+    typename ROSMessageType>
+  void
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
     std::vector<uint64_t> subscription_ids)
   {
-    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<MessageT, Alloc, Deleter>
-          >(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");
-        }
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
 
-        subscription->provide_intra_process_message(message);
-      } else {
-        subscriptions_.erase(id);
-      }
-    }
-  }
-
-  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);
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
 
     for (auto id : subscription_ids) {
       auto subscription_it = subscriptions_.find(id);
@@ -599,36 +362,62 @@ private:
         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 {
+      if (subscription_base == nullptr) {
         subscriptions_.erase(id);
+        continue;
+      }
+
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
+        subscription->provide_intra_process_data(message);
+        continue;
+      }
+
+      auto ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
+      }
+
+      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+        ROSMessageType ros_msg;
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, ros_msg);
+        ros_message_subscription->provide_intra_process_message(
+          std::make_shared<ROSMessageType>(ros_msg));
+      } else {
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          ros_message_subscription->provide_intra_process_message(message);
+        } else {
+          if constexpr (std::is_same<typename rclcpp::TypeAdapter<MessageT,
+            ROSMessageType>::ros_message_type, ROSMessageType>::value)
+          {
+            ROSMessageType ros_msg;
+            rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(
+              *message, ros_msg);
+            ros_message_subscription->provide_intra_process_message(
+              std::make_shared<ROSMessageType>(ros_msg));
+          }
+        }
       }
     }
   }
 
   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,
+    typename ROSMessageType>
+  void
   add_owned_msg_to_buffers(
     std::unique_ptr<MessageT, Deleter> message,
     std::vector<uint64_t> subscription_ids,
@@ -637,107 +426,84 @@ private:
     using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
     using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
 
+    using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+    using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+    using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+    using PublishedType = typename rclcpp::TypeAdapter<MessageT>::custom_type;
+    using PublishedTypeAllocatorTraits = allocator::AllocRebind<PublishedType, Alloc>;
+    using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type;
+    using PublishedTypeDeleter = allocator::Deleter<PublishedTypeAllocator, PublishedType>;
+
     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<MessageT, Alloc, Deleter>
-          >(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 (subscription_base == nullptr) {
+        subscriptions_.erase(subscription_it);
+        continue;
+      }
 
+      auto subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcessBuffer<PublishedType,
+        PublishedTypeAllocator, PublishedTypeDeleter, ROSMessageType>
+        >(subscription_base);
+      if (subscription != nullptr) {
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
-          subscription->provide_intra_process_message(std::move(message));
+          subscription->provide_intra_process_data(std::move(message));
         } else {
           // 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));
+          subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter)));
         }
-      } else {
-        subscriptions_.erase(subscription_it);
+
+        continue;
       }
-    }
-  }
 
-  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 ros_message_subscription = std::dynamic_pointer_cast<
+        rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+        ROSMessageTypeAllocator, ROSMessageTypeDeleter>
+        >(subscription_base);
+      if (nullptr == ros_message_subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, or to "
+                "SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
+                "ROSMessageTypeDeleter> which can happen when the publisher and "
+                "subscription use different allocator types, which is not supported");
       }
-      auto subscription_base = subscription_it->second.lock();
-      if (subscription_base) {
-        auto subscription = std::dynamic_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcessBuffer<ROSMessageType,
-          AllocatorT, ROSMessageTypeDeleter>
-          >(subscription_base);
-        if (nullptr == subscription) {
-          throw std::runtime_error(
-                  "failed to dynamic cast SubscriptionIntraProcessBase to "
-                  "SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>, which "
-                  "can happen when the publisher and subscription use different "
-                  "allocator types, which is not supported");
-        }
 
-        if (unique_ros_msg == nullptr) {
-          auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1);
-          ROSMessageTypeAllocatorTraits::construct(allocator, ptr);
-          unique_ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
-          rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *unique_ros_msg);
-        }
-
-        if (std::next(it) == subscription_ids.end()) {
-          // If this is the last subscription, give up ownership
-          subscription->provide_intra_process_message(std::move(unique_ros_msg));
-        } else {
-          // Copy the message since we have additional subscriptions to serve
-          using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
-          MessageUniquePtr copy_message;
-          auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1);
-          ROSMessageTypeAllocatorTraits::construct(allocator, ptr, *unique_ros_msg);
-          copy_message = MessageUniquePtr(ptr, deleter);
-
-          subscription->provide_intra_process_message(std::move(copy_message));
-        }
+      if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
+        ROSMessageTypeAllocator ros_message_alloc(allocator);
+        auto ptr = ros_message_alloc.allocate(1);
+        ros_message_alloc.construct(ptr);
+        ROSMessageTypeDeleter deleter;
+        allocator::set_allocator_for_deleter(&deleter, &allocator);
+        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *ptr);
+        auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
+        ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
       } else {
-        subscriptions_.erase(subscription_it);
+        if constexpr (std::is_same<MessageT, ROSMessageType>::value) {
+          if (std::next(it) == subscription_ids.end()) {
+            // If this is the last subscription, give up ownership
+            ros_message_subscription->provide_intra_process_message(std::move(message));
+          } else {
+            // Copy the message since we have additional subscriptions to serve
+            Deleter deleter = message.get_deleter();
+            allocator::set_allocator_for_deleter(&deleter, &allocator);
+            auto ptr = MessageAllocTraits::allocate(allocator, 1);
+            MessageAllocTraits::construct(allocator, ptr, *message);
+
+            ros_message_subscription->provide_intra_process_message(
+              std::move(MessageUniquePtr(ptr, deleter)));
+          }
+        }
       }
     }
   }

rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp

@@ -0,0 +1,68 @@
+// 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__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rcl/error_handling.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename RosMessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<void>
+>
+class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase
+{
+public:
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<RosMessageT, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, RosMessageT>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const RosMessageT>;
+  using MessageUniquePtr = std::unique_ptr<RosMessageT, ROSMessageTypeDeleter>;
+
+  SubscriptionROSMsgIntraProcessBuffer(
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile)
+  : SubscriptionIntraProcessBase(context, topic_name, qos_profile)
+  {}
+
+  virtual ~SubscriptionROSMsgIntraProcessBuffer()
+  {}
+
+  virtual void
+  provide_intra_process_message(ConstMessageSharedPtr message) = 0;
+
+  virtual void
+  provide_intra_process_message(MessageUniquePtr message) = 0;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -40,40 +40,47 @@ namespace experimental
 
 template<
   typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>,
-  typename CallbackMessageT = MessageT>
+  typename SubscribedType,
+  typename SubscribedTypeAlloc = std::allocator<SubscribedType>,
+  typename SubscribedTypeDeleter = std::default_delete<SubscribedType>,
+  typename ROSMessageType = SubscribedType,
+  typename Alloc = std::allocator<void>
+>
 class SubscriptionIntraProcess
   : public SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
   >
 {
   using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer<
-    MessageT,
-    Alloc,
-    Deleter
+    SubscribedType,
+    SubscribedTypeAlloc,
+    SubscribedTypeDeleter,
+    ROSMessageType
   >;
 
 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 MessageAllocTraits =
+    typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits;
+  using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator;
+  using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr;
+  using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr;
   using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr;
 
   SubscriptionIntraProcess(
-    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    AnySubscriptionCallback<MessageT, Alloc> callback,
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(
-      allocator,
+  : SubscriptionIntraProcessBuffer<SubscribedType, SubscribedTypeAlloc,
+      SubscribedTypeDeleter, ROSMessageType>(
+      std::make_shared<SubscribedTypeAlloc>(*allocator),
       context,
       topic_name,
       qos_profile,
@@ -95,15 +102,21 @@ public:
   virtual ~SubscriptionIntraProcess() = default;
 
   std::shared_ptr<void>
-  take_data()
+  take_data() override
   {
     ConstMessageSharedPtr shared_msg;
     MessageUniquePtr unique_msg;
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
     } else {
       unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
     }
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
@@ -112,9 +125,9 @@ public:
     );
   }
 
-  void execute(std::shared_ptr<void> & data)
+  void execute(std::shared_ptr<void> & data) override
   {
-    execute_impl<MessageT>(data);
+    execute_impl<SubscribedType>(data);
   }
 
 protected:
@@ -131,7 +144,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
     }
 
     rmw_message_info_t msg_info;
@@ -143,15 +156,15 @@ 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_;
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -15,15 +15,17 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <string>
 
-#include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rmw/impl/cpp/demangle.hpp"
 
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -36,34 +38,48 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
+  enum class EntityType : std::size_t
+  {
+    Subscription,
+  };
+
   RCLCPP_PUBLIC
   SubscriptionIntraProcessBase(
+    rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile)
-  : topic_name_(topic_name), qos_profile_(qos_profile)
+  : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile)
   {}
 
-  virtual ~SubscriptionIntraProcessBase() = default;
+  RCLCPP_PUBLIC
+  virtual ~SubscriptionIntraProcessBase();
 
   RCLCPP_PUBLIC
   size_t
-  get_number_of_ready_guard_conditions() {return 1;}
+  get_number_of_ready_guard_conditions() override {return 1;}
 
   RCLCPP_PUBLIC
-  bool
-  add_to_wait_set(rcl_wait_set_t * wait_set);
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
-  virtual bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override = 0;
+
+  std::shared_ptr<void>
+  take_data() override = 0;
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
+  void
+  execute(std::shared_ptr<void> & data) override = 0;
 
   virtual
-  std::shared_ptr<void>
-  take_data() = 0;
-
-  virtual void
-  execute(std::shared_ptr<void> & data) = 0;
-
-  virtual bool
+  bool
   use_take_shared_method() const = 0;
 
   RCLCPP_PUBLIC
@@ -74,14 +90,108 @@ public:
   QoS
   get_actual_qos() const;
 
-protected:
-  std::recursive_mutex reentrant_mutex_;
-  rcl_guard_condition_t gc_;
+  /// Set a callback to be called when each new message arrives.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bound to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called when a new message is received.
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Subscription));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::SubscriptionIntraProcessBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::SubscriptionIntraProcessBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    on_new_message_callback_ = new_callback;
+
+    if (unread_count_ > 0) {
+      if (qos_profile_.history() == HistoryPolicy::KeepAll) {
+        on_new_message_callback_(unread_count_);
+      } else {
+        // Use qos profile depth as upper bound for unread_count_
+        on_new_message_callback_(std::min(unread_count_, qos_profile_.depth()));
+      }
+      unread_count_ = 0;
+    }
+  }
+
+  /// Unset the callback registered for new messages, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    on_new_message_callback_ = nullptr;
+  }
+
+protected:
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_ {nullptr};
+  size_t unread_count_{0};
+  rclcpp::GuardCondition gc_;
 
-private:
   virtual void
   trigger_guard_condition() = 0;
 
+  void
+  invoke_on_new_message()
+  {
+    std::lock_guard<std::recursive_mutex> lock(this->callback_mutex_);
+    if (this->on_new_message_callback_) {
+      this->on_new_message_callback_(1);
+    } else {
+      this->unread_count_++;
+    }
+  }
+
+private:
   std::string topic_name_;
   QoS qos_profile_;
 };

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -16,8 +16,8 @@
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 
 #include <memory>
-#include <stdexcept>
 #include <string>
+#include <stdexcept>
 #include <utility>
 
 #include "rcl/error_handling.h"
@@ -27,6 +27,7 @@
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
@@ -36,24 +37,39 @@ namespace experimental
 {
 
 template<
-  typename MessageT,
-  typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>
+  typename SubscribedType,
+  typename Alloc = std::allocator<SubscribedType>,
+  typename Deleter = std::default_delete<SubscribedType>,
+  /// MessageT::ros_message_type if MessageT is a TypeAdapter,
+  /// otherwise just MessageT.
+  typename ROSMessageType = SubscribedType
 >
-class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
+class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer<ROSMessageType,
+    typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    allocator::Deleter<typename allocator::AllocRebind<ROSMessageType, Alloc>::allocator_type,
+    ROSMessageType>>
 {
 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 SubscribedTypeAllocatorTraits = allocator::AllocRebind<SubscribedType, Alloc>;
+  using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type;
+  using SubscribedTypeDeleter = allocator::Deleter<SubscribedTypeAllocator, SubscribedType>;
+
+  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
+  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
+  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
+
+  using ConstMessageSharedPtr = std::shared_ptr<const ROSMessageType>;
+  using MessageUniquePtr = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
+
+  using ConstDataSharedPtr = std::shared_ptr<const SubscribedType>;
+  using SubscribedTypeUniquePtr = std::unique_ptr<SubscribedType, SubscribedTypeDeleter>;
 
   using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
-    MessageT,
+    SubscribedType,
     Alloc,
-    Deleter
+    SubscribedTypeDeleter
     >::UniquePtr;
 
   SubscriptionIntraProcessBuffer(
@@ -62,78 +78,101 @@ public:
     const std::string & topic_name,
     const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(topic_name, qos_profile)
+  : SubscriptionROSMsgIntraProcessBuffer<ROSMessageType, ROSMessageTypeAllocator,
+      ROSMessageTypeDeleter>(
+      context, topic_name, qos_profile),
+    subscribed_type_allocator_(*allocator)
   {
+    allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_);
+
     // Create the intra-process buffer.
-    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
+    buffer_ = rclcpp::experimental::create_intra_process_buffer<SubscribedType, Alloc,
+        SubscribedTypeDeleter>(
       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);
-    }
+      std::make_shared<Alloc>(subscribed_type_allocator_));
   }
 
   bool
-  is_ready(rcl_wait_set_t * wait_set)
+  is_ready(rcl_wait_set_t * wait_set) override
   {
     (void) wait_set;
     return buffer_->has_data();
   }
 
+  SubscribedTypeUniquePtr
+  convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg)
+  {
+    if constexpr (!std::is_same<SubscribedType, ROSMessageType>::value) {
+      auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1);
+      SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr);
+      rclcpp::TypeAdapter<SubscribedType, ROSMessageType>::convert_to_custom(msg, *ptr);
+      return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_);
+    } else {
+      throw std::runtime_error(
+              "convert_ros_message_to_subscribed_type_unique_ptr "
+              "unexpectedly called without TypeAdapter");
+    }
+  }
+
   void
-  provide_intra_process_message(ConstMessageSharedPtr message)
+  provide_intra_process_message(ConstMessageSharedPtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_shared(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_message(MessageUniquePtr message) override
+  {
+    if constexpr (std::is_same<SubscribedType, ROSMessageType>::value) {
+      buffer_->add_unique(std::move(message));
+      trigger_guard_condition();
+    } else {
+      buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message));
+      trigger_guard_condition();
+    }
+    this->invoke_on_new_message();
+  }
+
+  void
+  provide_intra_process_data(ConstDataSharedPtr message)
   {
     buffer_->add_shared(std::move(message));
     trigger_guard_condition();
+    this->invoke_on_new_message();
   }
 
   void
-  provide_intra_process_message(MessageUniquePtr message)
+  provide_intra_process_data(SubscribedTypeUniquePtr message)
   {
     buffer_->add_unique(std::move(message));
     trigger_guard_condition();
+    this->invoke_on_new_message();
   }
 
   bool
-  use_take_shared_method() const
+  use_take_shared_method() const override
   {
     return buffer_->use_take_shared_method();
   }
 
 protected:
   void
-  trigger_guard_condition()
+  trigger_guard_condition() override
   {
-    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");
-    }
+    this->gc_.trigger();
   }
 
   BufferUniquePtr buffer_;
+  SubscribedTypeAllocator subscribed_type_allocator_;
+  SubscribedTypeDeleter subscribed_type_deleter_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/function_traits.hpp

@@ -80,10 +80,12 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 
 // std::bind for object methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
@@ -97,10 +99,12 @@ struct function_traits<
 
 // std::bind for object const methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...) const>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...) const>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
@@ -114,7 +118,9 @@ struct function_traits<
 
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
-#if defined _LIBCPP_VERSION  // libc++ (Clang)
+#if defined DOXYGEN_ONLY
+struct function_traits<std::bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>

rclcpp/include/rclcpp/generic_publisher.hpp

@@ -31,6 +31,8 @@
 #include "rclcpp/typesupport_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rmw/rmw.h"
+
 namespace rclcpp
 {
 
@@ -60,7 +62,6 @@ public:
    * \param topic_name Topic name
    * \param topic_type Topic type
    * \param qos %QoS settings
-   * \param callback Callback for new messages of serialized form
    * \param options %Publisher options.
    * Not all publisher options are currently respected, the only relevant options for this
    * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
@@ -117,9 +118,24 @@ public:
   RCLCPP_PUBLIC
   void publish(const rclcpp::SerializedMessage & message);
 
+  /**
+   * Publish a rclcpp::SerializedMessage via loaned message after de-serialization.
+   *
+   * \param message a serialized message
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can show
+   */
+  RCLCPP_PUBLIC
+  void publish_as_loaned_msg(const rclcpp::SerializedMessage & message);
+
 private:
   // The type support library should stay loaded, so it is stored in the GenericPublisher
   std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+
+  void * borrow_loaned_message();
+  void deserialize_message(
+    const rmw_serialized_message_t & serialized_message,
+    void * deserialized_msg);
+  void publish_loaned_message(void * loaned_message);
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/get_message_type_support_handle.hpp

@@ -28,7 +28,17 @@
 namespace rclcpp
 {
 
-/// Specialization for when MessageT is an actual ROS message type.
+#ifdef DOXYGEN_ONLY
+
+/// Returns the message type support for the given `MessageT` type.
+/**
+ * \tparam MessageT an actual ROS message type or an adapted type using `rclcpp::TypeAdapter`
+ */
+template<typename MessageT>
+constexpr const rosidl_message_type_support_t & get_message_type_support_handle();
+
+#else
+
 template<typename MessageT>
 constexpr
 typename std::enable_if_t<
@@ -44,7 +54,6 @@ get_message_type_support_handle()
   return *handle;
 }
 
-/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter.
 template<typename AdaptedType>
 constexpr
 typename std::enable_if_t<
@@ -63,12 +72,9 @@ get_message_type_support_handle()
   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.
- */
+// 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
 >
@@ -85,6 +91,8 @@ get_message_type_support_handle()
           "should never be called");
 }
 
+#endif
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -24,6 +24,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -187,7 +188,7 @@ private:
   mutable std::mutex node_graph_interfaces_mutex_;
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
-  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition interrupt_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };
 

rclcpp/include/rclcpp/guard_condition.hpp

@@ -40,6 +40,8 @@ public:
    *   Defaults to using the global default context singleton.
    *   Shared ownership of the context is held with the guard condition until
    *   destruction.
+   * \param[in] guard_condition_options Optional guard condition options to be used.
+   *   Defaults to using the default guard condition options.
    * \throws std::invalid_argument if the context is nullptr.
    * \throws rclcpp::exceptions::RCLError based exceptions when underlying
    *   rcl functions fail.
@@ -47,7 +49,9 @@ public:
   RCLCPP_PUBLIC
   explicit GuardCondition(
     rclcpp::Context::SharedPtr context =
-    rclcpp::contexts::get_global_default_context());
+    rclcpp::contexts::get_global_default_context(),
+    rcl_guard_condition_options_t guard_condition_options =
+    rcl_guard_condition_get_default_options());
 
   RCLCPP_PUBLIC
   virtual
@@ -58,6 +62,11 @@ public:
   rclcpp::Context::SharedPtr
   get_context() const;
 
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t &
+  get_rcl_guard_condition();
+
   /// Return the underlying rcl guard condition structure.
   RCLCPP_PUBLIC
   const rcl_guard_condition_t &
@@ -89,10 +98,27 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Adds the guard condition to a waitset
+  /**
+   * This function is thread-safe.
+   * \param[in] wait_set pointer to a wait set where to add the guard condition
+   */
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set);
+
+  RCLCPP_PUBLIC
+  void
+  set_on_trigger_callback(std::function<void(size_t)> callback);
+
 protected:
   rclcpp::Context::SharedPtr context_;
   rcl_guard_condition_t rcl_guard_condition_;
   std::atomic<bool> in_use_by_wait_set_{false};
+  std::recursive_mutex reentrant_mutex_;
+  std::function<void(size_t)> on_trigger_callback_{nullptr};
+  size_t unread_count_{0};
+  rcl_wait_set_t * wait_set_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/logger.hpp

@@ -79,7 +79,7 @@ get_node_logger(const rcl_node_t * node);
 /// Get the current logging directory.
 /**
  * For more details of how the logging directory is determined,
- * see \ref rcl_logging_get_logging_directory.
+ * see rcl_logging_get_logging_directory().
  *
  * \returns the logging directory being used.
  * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -64,9 +64,11 @@ public:
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
-  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0;
 
-  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+  virtual void
+  remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0;
 
   virtual void
   get_next_subscription(
@@ -98,52 +100,52 @@ public:
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+    const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
   get_service_by_handle(
-    std::shared_ptr<const rcl_service_t> service_handle,
+    const std::shared_ptr<const rcl_service_t> & service_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::ClientBase::SharedPtr
   get_client_by_handle(
-    std::shared_ptr<const rcl_client_t> client_handle,
+    const std::shared_ptr<const rcl_client_t> & client_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::TimerBase::SharedPtr
   get_timer_by_handle(
-    std::shared_ptr<const rcl_timer_t> timer_handle,
+    const std::shared_ptr<const rcl_timer_t> & timer_handle,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
-    rclcpp::CallbackGroup::SharedPtr group,
+    const rclcpp::CallbackGroup::SharedPtr & group,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription,
+    const rclcpp::SubscriptionBase::SharedPtr & subscription,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::ServiceBase::SharedPtr service,
+    const rclcpp::ServiceBase::SharedPtr & service,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_client(
-    rclcpp::ClientBase::SharedPtr client,
+    const rclcpp::ClientBase::SharedPtr & client,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(
-    rclcpp::TimerBase::SharedPtr timer,
+    const rclcpp::TimerBase::SharedPtr & timer,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   static rclcpp::CallbackGroup::SharedPtr
   get_group_by_waitable(
-    rclcpp::Waitable::SharedPtr waitable,
+    const rclcpp::Waitable::SharedPtr & waitable,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 };
 

rclcpp/include/rclcpp/node.hpp

@@ -395,22 +395,6 @@ public:
     rcl_interfaces::msg::ParameterDescriptor{},
     bool ignore_override = false);
 
-  /// Declare a parameter
-  [[deprecated(
-    "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
-    "If you want to declare a parameter that won't change type without a default value use:\n" \
-    "`node->declare_parameter<ParameterT>(name)`, where e.g. ParameterT=int64_t.\n\n" \
-    "If you want to declare a parameter that can dynamically change type use:\n" \
-    "```\n" \
-    "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
-    "descriptor.dynamic_typing = true;\n" \
-    "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
-    "```"
-  )]]
-  RCLCPP_PUBLIC
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name);
-
   /// Declare and initialize a parameter with a type.
   /**
    * See the non-templated declare_parameter() on this class for details.
@@ -720,6 +704,24 @@ public:
     ParameterT & parameter,
     const ParameterT & alternative_value) const;
 
+  /// Return the parameter value, or the "alternative_value" if not set.
+  /**
+   * If the parameter was not set, then the "alternative_value" argument is returned.
+   *
+   * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception.
+   *
+   * In all cases, the parameter is never set or declared within the node.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns The value of the parameter.
+   */
+  template<typename ParameterT>
+  ParameterT
+  get_parameter_or(
+    const std::string & name,
+    const ParameterT & alternative_value) const;
+
   /// Return the parameters by the given parameter names.
   /**
    * Like get_parameters(), this method may throw the

rclcpp/include/rclcpp/node_impl.hpp

@@ -220,12 +220,16 @@ Node::declare_parameter(
   // get advantage of parameter value template magic to get
   // the correct rclcpp::ParameterType from ParameterT
   rclcpp::ParameterValue value{ParameterT{}};
-  return this->declare_parameter(
-    name,
-    value.get_type(),
-    parameter_descriptor,
-    ignore_override
-  ).get<ParameterT>();
+  try {
+    return this->declare_parameter(
+      name,
+      value.get_type(),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException &) {
+    throw exceptions::UninitializedStaticallyTypedParameterException(name);
+  }
 }
 
 template<typename ParameterT>
@@ -309,6 +313,17 @@ Node::get_parameter_or(
   return got_parameter;
 }
 
+template<typename ParameterT>
+ParameterT
+Node::get_parameter_or(
+  const std::string & name,
+  const ParameterT & alternative_value) const
+{
+  ParameterT parameter;
+  get_parameter_or(name, parameter, alternative_value);
+  return parameter;
+}
+
 // this is a partially-specialized version of get_parameter above,
 // where our concrete type for ParameterT is std::map, but the to-be-determined
 // type is the value in the map.

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -34,7 +34,7 @@ namespace node_interfaces
 {
 
 /// Implementation of the NodeBase part of the Node API.
-class NodeBase : public NodeBaseInterface
+class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<NodeBase>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
@@ -114,13 +114,9 @@ public:
   get_associated_with_executor_atomic() override;
 
   RCLCPP_PUBLIC
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() override;
 
-  RCLCPP_PUBLIC
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const override;
-
   RCLCPP_PUBLIC
   bool
   get_use_intra_process_default() const override;
@@ -149,7 +145,7 @@ private:
 
   /// Guard condition for notifying the Executor of changes to this node.
   mutable std::recursive_mutex notify_guard_condition_mutex_;
-  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rclcpp::GuardCondition notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -24,6 +24,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -142,24 +143,17 @@ public:
   std::atomic_bool &
   get_associated_with_executor_atomic() = 0;
 
-  /// Return guard condition that should be notified when the internal node state changes.
+  /// Return a guard condition that should be notified when the internal node state changes.
   /**
    * For example, this should be notified when a publisher is added or removed.
    *
-   * \return the rcl_guard_condition_t if it is valid, else nullptr
+   * \return the GuardCondition if it is valid, else thow runtime error
    */
   RCLCPP_PUBLIC
   virtual
-  rcl_guard_condition_t *
+  rclcpp::GuardCondition &
   get_notify_guard_condition() = 0;
 
-  /// Acquire and return a scoped lock that protects the notify guard condition.
-  /** This should be used when triggering the notify guard condition. */
-  RCLCPP_PUBLIC
-  virtual
-  std::unique_lock<std::recursive_mutex>
-  acquire_notify_guard_condition_lock() const = 0;
-
   /// Return the default preference for using intra process communication.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -357,6 +357,8 @@ public:
   /// Return the topic endpoint information about publishers on a given topic.
   /**
    * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_publishers_info_by_topic
    */
   RCLCPP_PUBLIC
@@ -367,6 +369,8 @@ public:
   /// Return the topic endpoint information about subscriptions on a given topic.
   /**
    * \param[in] topic_name the actual topic name used; it will not be automatically remapped.
+   * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name,
+   *   otherwise it should be a valid ROS topic name.
    * \sa rclcpp::Node::get_subscriptions_info_by_topic
    */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -103,25 +103,6 @@ public:
   virtual
   ~NodeParameters();
 
-// This is overriding a deprecated method, so we need to ignore the deprecation warning here.
-// Users of the method will still get a warning!
-#ifndef _WIN32
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
-  RCLCPP_PUBLIC
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name) override;
-#ifndef _WIN32
-# pragma GCC diagnostic pop
-#else
-# pragma warning(pop)
-#endif
-
   RCLCPP_PUBLIC
   const rclcpp::ParameterValue &
   declare_parameter(

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -45,17 +45,6 @@ struct OnSetParametersCallbackHandle
   OnParametersSetCallbackType callback;
 };
 
-#define RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE \
-  "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
-  "If you want to declare a parameter that won't change type without a default value use:\n" \
-  "`node_params->declare_parameter(name, type)`, with e.g. type=rclcpp::PARAMETER_INTEGER.\n\n" \
-  "If you want to declare a parameter that can dynamically change type use:\n" \
-  "```\n" \
-  "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
-  "descriptor.dynamic_typing = true;\n" \
-  "node_params->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
-  "```"
-
 /// Pure virtual interface class for the NodeParameters part of the Node API.
 class NodeParametersInterface
 {
@@ -66,15 +55,6 @@ public:
   virtual
   ~NodeParametersInterface() = default;
 
-  /// Declare a parameter.
-  /**
-   * \sa rclcpp::Node::declare_parameter
-   */
-  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
-  virtual
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name) = 0;
-
   /// Declare and initialize a parameter.
   /**
    * \sa rclcpp::Node::declare_parameter

rclcpp/include/rclcpp/parameter.hpp

@@ -264,6 +264,7 @@ get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
 
 }  // namespace detail
 
+/// \cond
 template<typename T>
 decltype(auto)
 Parameter::get_value() const
@@ -275,6 +276,7 @@ Parameter::get_value() const
     throw exceptions::InvalidParameterTypeException(this->name_, ex.what());
   }
 }
+/// \endcond
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -79,7 +79,7 @@ public:
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
-  AsyncParametersClient(
+  explicit AsyncParametersClient(
     const std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
@@ -102,7 +102,7 @@ public:
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
-  AsyncParametersClient(
+  explicit AsyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
@@ -185,7 +185,7 @@ public:
   /**
    * This function filters the parameters to be set based on the node name.
    *
-   * \param yaml_filename the full name of the yaml file
+   * \param parameter_map named parameters to be loaded
    * \return the future of the set_parameter service used to load the parameters
    * \throw InvalidParametersException if there is no parameter to set
    */
@@ -333,7 +333,7 @@ public:
   {}
 
   template<typename NodeT>
-  SyncParametersClient(
+  explicit SyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -165,7 +165,7 @@ public:
    * \param[in] qos The QoS settings to use for any subscriptions.
    */
   template<typename NodeT>
-  ParameterEventHandler(
+  explicit ParameterEventHandler(
     NodeT node,
     const rclcpp::QoS & qos =
     rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))

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

@@ -83,7 +83,6 @@ public:
 
   /// 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>;
@@ -255,7 +254,7 @@ public:
       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,11 +264,11 @@ public:
       get_subscription_count() > get_intra_process_subscription_count();
 
     if (inter_process_publish_needed) {
-      auto shared_msg = this->do_intra_process_publish_and_return_shared<T, ROSMessageTypeDeleter>(
-        std::move(msg));
+      auto shared_msg =
+        this->do_intra_process_ros_message_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_ros_message_publish(std::move(msg));
     }
   }
 
@@ -322,29 +321,27 @@ public:
   >
   publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
   {
+    // Avoid allocating when not using intra process.
     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;
+      // In this case we're not using intra process.
+      ROSMessageType ros_msg;
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
+      return this->do_inter_process_publish(ros_msg);
     }
-    // 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);
+      ROSMessageType ros_msg;
+      // TODO(clalancette): This is unnecessarily doing an additional conversion
+      // that may have already been done in do_intra_process_publish_and_return_shared().
+      // We should just reuse that effort.
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
+      this->do_intra_process_publish(std::move(msg));
+      this->do_inter_process_publish(ros_msg);
     } else {
-      this->do_intra_process_publish<T, PublishedTypeDeleter>(std::move(msg));
+      this->do_intra_process_publish(std::move(msg));
     }
   }
 
@@ -366,7 +363,7 @@ public:
   >
   publish(const T & msg)
   {
-    // Avoid double allocation when not using intra process.
+    // Avoid double allocating when not using intra process.
     if (!intra_process_is_enabled_) {
       // Convert to the ROS message equivalent and publish it.
       ROSMessageType ros_msg;
@@ -374,6 +371,7 @@ public:
       // In this case we're not using intra process.
       return this->do_inter_process_publish(ros_msg);
     }
+
     // Otherwise we have to allocate memory in a unique_ptr 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.
@@ -503,9 +501,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<PublishedType, PublishedTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -516,17 +513,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<PublishedType, ROSMessageType, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_,
-      ros_message_type_deleter_,
       published_type_allocator_);
   }
 
-  template<typename T, typename DeleterT>
-  std::shared_ptr<const ROSMessageType>
-  do_intra_process_publish_and_return_shared(std::unique_ptr<T, DeleterT> msg)
+  void
+  do_intra_process_ros_message_publish(std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -537,15 +531,33 @@ 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,
+    ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      ros_message_type_allocator_);
+  }
+
+  std::shared_ptr<const ROSMessageType>
+  do_intra_process_ros_message_publish_and_return_shared(
+    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publish msg which is a null pointer");
+    }
+
+    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
              AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
-      ros_message_type_allocator_,
-      ros_message_type_deleter_,
-      published_type_allocator_);
+      ros_message_type_allocator_);
   }
 
+
   /// Return a new unique_ptr using the ROSMessageType of the publisher.
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
   create_ros_message_unique_ptr()
@@ -555,7 +567,7 @@ protected:
     return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
   }
 
-  /// Duplicate a given ROS message as a unique_ptr.
+  /// Duplicate a given ros message as a unique_ptr.
   std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
   duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg)
   {

rclcpp/include/rclcpp/publisher_base.hpp

@@ -18,11 +18,14 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
 #include <sstream>
 #include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include "rcl/publisher.h"
@@ -33,6 +36,7 @@
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/time.hpp"
 
 namespace rclcpp
 {
@@ -110,9 +114,10 @@ public:
   get_publisher_handle() const;
 
   /// Get all the QoS event handlers associated with this publisher.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get subscription count
@@ -203,6 +208,111 @@ public:
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Wait until all published messages are acknowledged or until the specified timeout elapses.
+  /**
+   * This method waits until all published messages are acknowledged by all matching
+   * subscriptions or the given timeout elapses.
+   *
+   * If the timeout is negative then this method will block indefinitely until all published
+   * messages are acknowledged.
+   * If the timeout is zero then this method will not block, it will check if all published
+   * messages are acknowledged and return immediately.
+   * If the timeout is greater than zero, this method will wait until all published messages are
+   * acknowledged or the timeout elapses.
+   *
+   * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE.
+   * Otherwise this method will immediately return `true`.
+   *
+   * \param[in] timeout the duration to wait for all published messages to be acknowledged.
+   * \return `true` if all published messages were acknowledged before the given timeout
+   *   elapsed, otherwise `false`.
+   * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs
+   * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or
+   *   less than std::chrono::nanoseconds::min()
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli>
+  bool
+  wait_for_all_acked(
+    std::chrono::duration<DurationRepT, DurationT> timeout =
+    std::chrono::duration<DurationRepT, DurationT>(-1)) const
+  {
+    rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count();
+
+    rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout);
+    if (ret == RCL_RET_OK) {
+      return true;
+    } else if (ret == RCL_RET_TIMEOUT) {
+      return false;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered publisher event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -216,7 +326,7 @@ protected:
       rcl_publisher_event_init,
       publisher_handle_,
       event_type);
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -226,7 +336,8 @@ protected:
 
   std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_publisher_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
     std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
@@ -235,6 +346,8 @@ protected:
   uint64_t intra_process_publisher_id_;
 
   rmw_gid_t rmw_gid_;
+
+  const rosidl_message_type_support_t type_support_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_options.hpp

@@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  PublisherOptionsWithAllocator<Allocator>() {}
+  PublisherOptionsWithAllocator() {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)

rclcpp/include/rclcpp/qos_event.hpp

@@ -17,16 +17,21 @@
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
 
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/incompatible_qos_events_statuses.h"
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -84,6 +89,11 @@ public:
 class QOSEventHandlerBase : public Waitable
 {
 public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
   RCLCPP_PUBLIC
   virtual ~QOSEventHandlerBase();
 
@@ -94,7 +104,7 @@ public:
 
   /// Add the Waitable to a wait set.
   RCLCPP_PUBLIC
-  bool
+  void
   add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
   /// Check if the Waitable is ready.
@@ -102,9 +112,111 @@ public:
   bool
   is_ready(rcl_wait_set_t * wait_set) override;
 
+  /// Set a callback to be called when each new event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_event_set_callback
+   * \sa rcl_event_set_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Event));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_event_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_event_callback_));
+  }
+
+  /// Unset the callback registered for new events, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
 protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
   rcl_event_t event_handle_;
   size_t wait_set_event_index_;
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
 };
 
 template<typename EventCallbackT, typename ParentHandleT>
@@ -117,9 +229,8 @@ public:
     InitFuncT init_func,
     ParentHandleT parent_handle,
     EventTypeEnum event_type)
-  : event_callback_(callback)
+  : parent_handle_(parent_handle), event_callback_(callback)
   {
-    parent_handle_ = parent_handle;
     event_handle_ = rcl_get_zero_initialized_event();
     rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
     if (ret != RCL_RET_OK) {
@@ -148,6 +259,13 @@ public:
     return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
   }
 
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
   /// Execute any entities of the Waitable that are ready.
   void
   execute(std::shared_ptr<void> & data) override

rclcpp/include/rclcpp/service.hpp

@@ -19,23 +19,30 @@
 #include <functional>
 #include <iostream>
 #include <memory>
+#include <mutex>
 #include <sstream>
 #include <string>
 
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
 #include "rcl/service.h"
 
-#include "rclcpp/any_service_callback.hpp"
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
+
 #include "tracetools/tracetools.h"
 
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+
 namespace rclcpp
 {
 
@@ -121,6 +128,123 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Get the actual response publisher 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 service, 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.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response publisher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_publisher_actual_qos() const;
+
+  /// Get the actual request subscription 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 service, 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.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new request is received.
+  /**
+   * The callback receives a size_t which is the number of requests received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if requests were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the service
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_service_set_on_new_request_callback
+   * \sa rcl_service_set_on_new_request_callback
+   *
+   * \param[in] callback functor to be called when a new request is received
+   */
+  void
+  set_on_new_request_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_request_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_requests) {
+        try {
+          callback(number_of_requests);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ServiceBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new request' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ServiceBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new request' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_request_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_request_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_request_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_request_callback_));
+  }
+
+  /// Unset the callback registered for new requests, if any.
+  void
+  clear_on_new_request_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_request_callback_) {
+      set_on_new_request_callback(nullptr, nullptr);
+      on_new_request_callback_ = nullptr;
+    }
+  }
+
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
@@ -132,12 +256,21 @@ protected:
   const rcl_node_t *
   get_rcl_node_handle() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data);
+
   std::shared_ptr<rcl_node_t> node_handle_;
 
   std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 
+  rclcpp::Logger node_logger_;
+
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_request_callback_{nullptr};
 };
 
 template<typename ServiceT>

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -21,6 +21,7 @@
 #include "rcl/allocator.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -61,17 +62,17 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  void add_guard_condition(const rcl_guard_condition_t * guard_condition) override
+  void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override
   {
     for (const auto & existing_guard_condition : guard_conditions_) {
-      if (existing_guard_condition == guard_condition) {
+      if (existing_guard_condition == &guard_condition) {
         return;
       }
     }
-    guard_conditions_.push_back(guard_condition);
+    guard_conditions_.push_back(&guard_condition);
   }
 
-  void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override
+  void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override
   {
     for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) {
       if (*it == guard_condition) {
@@ -163,30 +164,22 @@ public:
       if (!group || !group->can_be_taken_from().load()) {
         continue;
       }
-      group->find_subscription_ptrs_if(
+
+      group->collect_all_ptrs(
         [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
           subscription_handles_.push_back(subscription->get_subscription_handle());
-          return false;
-        });
-      group->find_service_ptrs_if(
+        },
         [this](const rclcpp::ServiceBase::SharedPtr & service) {
           service_handles_.push_back(service->get_service_handle());
-          return false;
-        });
-      group->find_client_ptrs_if(
+        },
         [this](const rclcpp::ClientBase::SharedPtr & client) {
           client_handles_.push_back(client->get_client_handle());
-          return false;
-        });
-      group->find_timer_ptrs_if(
+        },
         [this](const rclcpp::TimerBase::SharedPtr & timer) {
           timer_handles_.push_back(timer->get_timer_handle());
-          return false;
-        });
-      group->find_waitable_ptrs_if(
+        },
         [this](const rclcpp::Waitable::SharedPtr & waitable) {
           waitable_handles_.push_back(waitable);
-          return false;
         });
     }
 
@@ -203,7 +196,7 @@ public:
 
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override
   {
-    for (auto subscription : subscription_handles_) {
+    for (const std::shared_ptr<const rcl_subscription_t> & subscription : subscription_handles_) {
       if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -212,7 +205,7 @@ public:
       }
     }
 
-    for (auto client : client_handles_) {
+    for (const std::shared_ptr<const rcl_client_t> & client : client_handles_) {
       if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -221,7 +214,7 @@ public:
       }
     }
 
-    for (auto service : service_handles_) {
+    for (const std::shared_ptr<const rcl_service_t> & service : service_handles_) {
       if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -230,7 +223,7 @@ public:
       }
     }
 
-    for (auto timer : timer_handles_) {
+    for (const std::shared_ptr<const rcl_timer_t> & timer : timer_handles_) {
       if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
@@ -240,22 +233,11 @@ public:
     }
 
     for (auto guard_condition : guard_conditions_) {
-      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Couldn't add guard_condition to wait set: %s",
-          rcl_get_error_string().str);
-        return false;
-      }
+      detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition);
     }
 
-    for (auto waitable : waitable_handles_) {
-      if (!waitable->add_to_wait_set(wait_set)) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
-        return false;
-      }
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
+      waitable->add_to_wait_set(wait_set);
     }
     return true;
   }
@@ -412,7 +394,7 @@ public:
   {
     auto it = waitable_handles_.begin();
     while (it != waitable_handles_.end()) {
-      auto waitable = *it;
+      std::shared_ptr<Waitable> & waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
         auto group = get_group_by_waitable(waitable, weak_groups_to_nodes);
@@ -448,7 +430,7 @@ public:
   size_t number_of_ready_subscriptions() const override
   {
     size_t number_of_subscriptions = subscription_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
     }
     return number_of_subscriptions;
@@ -457,7 +439,7 @@ public:
   size_t number_of_ready_services() const override
   {
     size_t number_of_services = service_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_services += waitable->get_number_of_ready_services();
     }
     return number_of_services;
@@ -466,7 +448,7 @@ public:
   size_t number_of_ready_events() const override
   {
     size_t number_of_events = 0;
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_events += waitable->get_number_of_ready_events();
     }
     return number_of_events;
@@ -475,7 +457,7 @@ public:
   size_t number_of_ready_clients() const override
   {
     size_t number_of_clients = client_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_clients += waitable->get_number_of_ready_clients();
     }
     return number_of_clients;
@@ -484,7 +466,7 @@ public:
   size_t number_of_guard_conditions() const override
   {
     size_t number_of_guard_conditions = guard_conditions_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
     }
     return number_of_guard_conditions;
@@ -493,7 +475,7 @@ public:
   size_t number_of_ready_timers() const override
   {
     size_t number_of_timers = timer_handles_.size();
-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
       number_of_timers += waitable->get_number_of_ready_timers();
     }
     return number_of_timers;
@@ -509,7 +491,7 @@ private:
   using VectorRebind =
     std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
-  VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
+  VectorRebind<const rclcpp::GuardCondition *> guard_conditions_;
 
   VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
   VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;

rclcpp/include/rclcpp/subscription.hpp

@@ -146,19 +146,19 @@ public:
     options_(options),
     message_memory_strategy_(message_memory_strategy)
   {
-    if (options.event_callbacks.deadline_callback) {
+    if (options_.event_callbacks.deadline_callback) {
       this->add_event_handler(
-        options.event_callbacks.deadline_callback,
+        options_.event_callbacks.deadline_callback,
         RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
     }
-    if (options.event_callbacks.liveliness_callback) {
+    if (options_.event_callbacks.liveliness_callback) {
       this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
+        options_.event_callbacks.liveliness_callback,
         RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
     }
-    if (options.event_callbacks.incompatible_qos_callback) {
+    if (options_.event_callbacks.incompatible_qos_callback) {
       this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
+        options_.event_callbacks.incompatible_qos_callback,
         RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
     } else if (options_.use_default_callbacks) {
       // Register default callback when not specified
@@ -172,14 +172,14 @@ public:
         // pass
       }
     }
-    if (options.event_callbacks.message_lost_callback) {
+    if (options_.event_callbacks.message_lost_callback) {
       this->add_event_handler(
-        options.event_callbacks.message_lost_callback,
+        options_.event_callbacks.message_lost_callback,
         RCL_SUBSCRIPTION_MESSAGE_LOST);
     }
 
     // Setup intra process publishing if requested.
-    if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
+    if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // Check if the QoS is compatible with intra-process.
@@ -197,15 +197,23 @@ public:
                 "intraprocess communication allowed only with volatile durability");
       }
 
+      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
+        MessageT,
+        SubscribedType,
+        SubscribedTypeAllocator,
+        SubscribedTypeDeleter,
+        ROSMessageT,
+        AllocatorT>;
+
       // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
       auto context = node_base->get_context();
       subscription_intra_process_ = std::make_shared<SubscriptionIntraProcessT>(
         callback,
-        options.get_allocator(),
+        options_.get_allocator(),
         context,
         this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
         qos_profile,
-        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
+        resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),
@@ -276,7 +284,7 @@ public:
 
   /// Take the next message from the inter-process subscription.
   /**
-   * This verison takes a SubscribedType which is different frmo the
+   * This version takes a SubscribedType which is different from the
    * ROSMessageType when a rclcpp::TypeAdapter is in used.
    *
    * \sa take(ROSMessageType &, rclcpp::MessageInfo &)
@@ -355,11 +363,31 @@ public:
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
+    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<ROSMessageType *>(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;});
+
+    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.
@@ -404,13 +432,6 @@ private:
 
   /// Component which computes and publishes topic statistics for this subscriber
   SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
-
-  using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-    ROSMessageType,
-    AllocatorT,
-    ROSMessageTypeDeleter,
-    MessageT>;
-  std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -17,16 +17,20 @@
 
 #include <atomic>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
 #include <utility>
 
+#include "rcl/event_callback.h"
 #include "rcl/subscription.h"
 
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
@@ -35,6 +39,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -99,9 +104,10 @@ public:
   get_subscription_handle() const;
 
   /// Get all the QoS event handlers associated with this subscription.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get the actual QoS settings, after the defaults have been determined.
@@ -283,6 +289,243 @@ public:
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Set a callback to be called when each new message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_subscription_set_on_new_message_callback
+   * \sa rcl_subscription_set_on_new_message_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_message_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_messages) {
+        try {
+          callback(number_of_messages);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new message' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new message' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_message_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_message_callback_));
+  }
+
+  /// Unset the callback registered for new messages, if any.
+  void
+  clear_on_new_message_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    if (on_new_message_callback_) {
+      set_on_new_message_callback(nullptr, nullptr);
+      on_new_message_callback_ = nullptr;
+    }
+  }
+
+  /// Set a callback to be called when each new intra-process message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_intra_process_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_intra_process_message_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    subscription_intra_process_->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new intra-process messages, if any.
+  void
+  clear_on_new_intra_process_message_callback()
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    subscription_intra_process_->clear_on_ready_callback();
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered subscription event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
+  /// Check if content filtered topic feature of the subscription instance is enabled.
+  /**
+   * \return boolean flag indicating if the content filtered topic of this subscription is enabled.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_cft_enabled() const;
+
+  /// Set the filter expression and expression parameters for the subscription.
+  /**
+   * \param[in] filter_expression A filter expression to set.
+   *   \sa ContentFilterOptions::filter_expression
+   *   An empty string ("") will clear the content filter setting of the subscription.
+   * \param[in] expression_parameters Array of expression parameters to set.
+   *   \sa ContentFilterOptions::expression_parameters
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  void
+  set_content_filter(
+    const std::string & filter_expression,
+    const std::vector<std::string> & expression_parameters = {});
+
+  /// Get the filter expression and expression parameters for the subscription.
+  /**
+   * \return rclcpp::ContentFilterOptions The content filter options to get.
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ContentFilterOptions
+  get_content_filter() const;
+
 protected:
   template<typename EventCallbackT>
   void
@@ -297,7 +540,7 @@ protected:
       get_subscription_handle(),
       event_type);
     qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -307,17 +550,24 @@ protected:
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data);
+
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  rclcpp::Logger node_logger_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_subscription_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_subscription_id_;
+  std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
@@ -329,6 +579,9 @@ private:
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
   std::unordered_map<rclcpp::QOSEventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_content_filter_options.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_
+
+#include <string>
+#include <vector>
+
+namespace rclcpp
+{
+
+/// Options to configure content filtered topic in the subscription.
+struct ContentFilterOptions
+{
+  /// Filter expression is similar to the WHERE part of an SQL clause.
+  std::string filter_expression;
+  /**
+   * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0)
+   * in the filter_expression. The maximum expression_parameters size is 100.
+   */
+  std::vector<std::string> expression_parameters;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription_options.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -81,6 +82,8 @@ struct SubscriptionOptionsBase
   TopicStatisticsOptions topic_stats_options;
 
   QosOverridingOptions qos_overriding_options;
+
+  ContentFilterOptions content_filter_options;
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -94,7 +97,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  SubscriptionOptionsWithAllocator<Allocator>() {}
+  SubscriptionOptionsWithAllocator() {}
 
   /// Constructor using base class as input.
   explicit SubscriptionOptionsWithAllocator(
@@ -123,6 +126,21 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
       rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options);
     }
 
+    // Copy content_filter_options into rcl_subscription_options.
+    if (!content_filter_options.filter_expression.empty()) {
+      std::vector<const char *> cstrings =
+        get_c_vector_string(content_filter_options.expression_parameters);
+      rcl_ret_t ret = rcl_subscription_options_set_content_filter_options(
+        get_c_string(content_filter_options.filter_expression),
+        cstrings.size(),
+        cstrings.data(),
+        &result);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "failed to set content_filter_options");
+      }
+    }
+
     return result;
   }
 

rclcpp/include/rclcpp/time_source.hpp

@@ -56,6 +56,7 @@ public:
    *
    * \param node std::shared pointer to a initialized node
    * \param qos QoS that will be used when creating a `/clock` subscription.
+   * \param use_clock_thread whether to spin the attached node in a separate thread
    */
   RCLCPP_PUBLIC
   explicit TimeSource(
@@ -68,6 +69,7 @@ public:
    * An Empty TimeSource class
    *
    * \param qos QoS that will be used when creating a `/clock` subscription.
+   * \param use_clock_thread whether to spin the attached node in a separate thread.
    */
   RCLCPP_PUBLIC
   explicit TimeSource(
@@ -145,9 +147,6 @@ public:
   ~TimeSource();
 
 private:
-  class ClocksState;
-  std::shared_ptr<ClocksState> clocks_state_;
-
   class NodeState;
   std::shared_ptr<NodeState> node_state_;
 

rclcpp/include/rclcpp/timer.hpp

@@ -113,7 +113,8 @@ public:
 
   /// Check how long the timer has until its next scheduled callback.
   /**
-   * \return A std::chrono::duration representing the relative time until the next callback.
+   * \return A std::chrono::duration representing the relative time until the next callback
+   * or std::chrono::nanoseconds::max() if the timer is canceled.
    * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure
    */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/utilities.hpp

@@ -65,13 +65,16 @@ enum class SignalHandlerOptions
  *
  * \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.
+ * \param[in] argc number of command-line arguments to parse.
+ * \param[in] argv array of command-line arguments to parse.
+ * \param[in] init_options initialization options to apply.
+ * \param[in] signal_handler_options option to indicate which signal handlers should be installed.
  */
 RCLCPP_PUBLIC
 void
 init(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const InitOptions & init_options = InitOptions(),
   SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);
 
@@ -87,7 +90,7 @@ init(
  *
  * This function is thread-safe.
  *
- * \param signal_handler_options option to indicate which signal handlers should be installed.
+ * \param[in] 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
@@ -136,7 +139,7 @@ RCLCPP_PUBLIC
 std::vector<std::string>
 init_and_remove_ros_arguments(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const InitOptions & init_options = InitOptions());
 
 /// Remove ROS-specific arguments from argument vector.
@@ -154,7 +157,7 @@ init_and_remove_ros_arguments(
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
-remove_ros_arguments(int argc, char const * const argv[]);
+remove_ros_arguments(int argc, char const * const * argv);
 
 /// Check rclcpp's status.
 /**
@@ -318,6 +321,15 @@ RCLCPP_PUBLIC
 const char *
 get_c_string(const std::string & string_in);
 
+/// Return the std::vector of C string from the given std::vector<std::string>.
+/**
+ * \param[in] strings_in is a std::vector of std::string
+ * \return the std::vector of C string from the std::vector<std::string>
+ */
+RCLCPP_PUBLIC
+std::vector<const char *>
+get_c_vector_string(const std::vector<std::string> & strings_in);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 #include <string>
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/wait_set.hpp"
@@ -54,6 +56,7 @@ bool wait_for_message(
 
   rclcpp::WaitSet wait_set;
   wait_set.add_subscription(subscription);
+  RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); );
   wait_set.add_guard_condition(gc);
   auto ret = wait_set.wait(time_to_wait);
   if (ret.kind() != rclcpp::WaitResultKind::Ready) {

rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp

@@ -383,10 +383,7 @@ protected:
         continue;
       }
       rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
-      bool successful = waitable.add_to_wait_set(&rcl_wait_set_);
-      if (!successful) {
-        throw std::runtime_error("waitable unexpectedly failed to be added to wait set");
-      }
+      waitable.add_to_wait_set(&rcl_wait_set_);
     }
   }
 

rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp

@@ -191,7 +191,7 @@ protected:
 
     WritePreferringReadWriteLock & parent_lock_;
 
-    friend WritePreferringReadWriteLock;
+    friend class WritePreferringReadWriteLock;
   };
 
   /// Write mutex for the WritePreferringReadWriteLock.
@@ -212,7 +212,7 @@ protected:
 
     WritePreferringReadWriteLock & parent_lock_;
 
-    friend WritePreferringReadWriteLock;
+    friend class WritePreferringReadWriteLock;
   };
 
   /// Return read mutex which can be used with standard constructs like std::lock_guard.

rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp

@@ -203,7 +203,7 @@ protected:
     std::shared_ptr<rclcpp::Waitable> && waitable,
     std::shared_ptr<void> && associated_entity,
     std::function<
-      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
+      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void> &&)
     > add_waitable_function)
   {
     // Explicitly no thread synchronization.

rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp

@@ -216,7 +216,7 @@ protected:
     std::shared_ptr<rclcpp::Waitable> && waitable,
     std::shared_ptr<void> && associated_entity,
     std::function<
-      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
+      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void> &&)
     > add_waitable_function)
   {
     using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -155,7 +155,8 @@ public:
           this->storage_add_subscription(std::move(local_subscription));
         }
         if (mask.include_events) {
-          for (auto event : inner_subscription->get_event_handlers()) {
+          for (auto key_event_pair : inner_subscription->get_event_handlers()) {
+            auto event = key_event_pair.second;
             auto local_subscription = inner_subscription;
             bool already_in_use =
             local_subscription->exchange_in_use_by_wait_set_state(event.get(), true);
@@ -225,7 +226,8 @@ public:
           this->storage_remove_subscription(std::move(local_subscription));
         }
         if (mask.include_events) {
-          for (auto event : inner_subscription->get_event_handlers()) {
+          for (auto key_event_pair : inner_subscription->get_event_handlers()) {
+            auto event = key_event_pair.second;
             auto local_subscription = inner_subscription;
             local_subscription->exchange_in_use_by_wait_set_state(event.get(), false);
             this->storage_remove_waitable(std::move(event));

rclcpp/include/rclcpp/waitable.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__WAITABLE_HPP_
 
 #include <atomic>
+#include <functional>
 #include <memory>
 
 #include "rclcpp/macros.hpp"
@@ -103,12 +104,11 @@ public:
   /// Add the Waitable to a wait set.
   /**
    * \param[in] wait_set A handle to the wait set to add the Waitable to.
-   * \return `true` if the Waitable is added successfully, `false` otherwise.
    * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*()
    */
   RCLCPP_PUBLIC
   virtual
-  bool
+  void
   add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
 
   /// Check if the Waitable is ready.
@@ -145,8 +145,7 @@ public:
    * ```cpp
    * // ... create a wait set and a Waitable
    * // Add the Waitable to the wait set
-   * bool add_ret = waitable.add_to_wait_set(wait_set);
-   * // ... error handling
+   * waitable.add_to_wait_set(wait_set);
    * // Wait
    * rcl_ret_t wait_ret = rcl_wait(wait_set);
    * // ... error handling
@@ -161,6 +160,24 @@ public:
   std::shared_ptr<void>
   take_data() = 0;
 
+  /// Take the data so that it can be consumed with `execute`.
+  /**
+   * This function allows to specify an entity ID to take the data from.
+   * Entity IDs are identifiers that can be defined by waitable-derived
+   * classes that are composed of several distinct entities.
+   * The main use-case is in conjunction with the listener APIs.
+   *
+   * \param[in] id the id of the entity from which to take
+   * \returns the type-erased data taken from entity specified
+   *
+   * \sa rclcpp::Waitable::take_data
+   * \sa rclcpp::Waitable::set_on_ready_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id);
+
   /// Execute data that is passed in.
   /**
    * Before calling this method, the Waitable should be added to a wait set,
@@ -172,8 +189,7 @@ public:
    * ```cpp
    * // ... create a wait set and a Waitable
    * // Add the Waitable to the wait set
-   * bool add_ret = waitable.add_to_wait_set(wait_set);
-   * // ... error handling
+   * waitable.add_to_wait_set(wait_set);
    * // Wait
    * rcl_ret_t wait_ret = rcl_wait(wait_set);
    * // ... error handling
@@ -203,6 +219,45 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Set a callback to be called whenever the waitable becomes ready.
+  /**
+   * The callback receives a size_t which is the number of times the waitable was ready
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if waitable was triggered before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Note: this function must be overridden with a proper implementation
+   * by the custom classes who inherit from rclcpp::Waitable if they want to use it.
+   *
+   * \sa rclcpp::Waitable::clear_on_ready_callback
+   *
+   * \param[in] callback functor to be called when the waitable becomes ready
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback);
+
+  /// Unset any callback registered via set_on_ready_callback.
+  /**
+   * Note: this function must be overridden with a proper implementation
+   * by the custom classes who inherit from rclcpp::Waitable if they want to use it.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  clear_on_ready_callback();
+
 private:
   std::atomic<bool> in_use_by_wait_set_{false};
 };  // class Waitable

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>13.1.0</version>
+  <version>16.0.4</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>
@@ -39,6 +39,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/resource/logging.hpp.em

@@ -141,8 +141,8 @@ def get_rclcpp_suffix_from_features(features):
     }; \
 @[ end if] \
 @[ if 'stream' in feature_combination]@
-    std::stringstream ss; \
-    ss << @(stream_arg); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << @(stream_arg); \
 @[ end if]@
     RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \
 @{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@
@@ -153,7 +153,7 @@ def get_rclcpp_suffix_from_features(features):
 @[ if 'stream' not in feature_combination]@
       __VA_ARGS__); \
 @[ else]@
-      "%s", ss.str().c_str()); \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
 @[ end if]@
   } while (0)
 

rclcpp/src/rclcpp/callback_group.cpp

@@ -12,9 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/callback_group.hpp"
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
 
-#include <vector>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/waitable.hpp"
 
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
@@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup(
   automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
+CallbackGroup::~CallbackGroup()
+{
+  trigger_notify_guard_condition();
+}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -40,6 +54,51 @@ CallbackGroup::type() const
   return type_;
 }
 
+void CallbackGroup::collect_all_ptrs(
+  std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+  std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+  std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+  std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+  std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const rclcpp::SubscriptionBase::WeakPtr & weak_ptr : subscription_ptrs_) {
+    rclcpp::SubscriptionBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      sub_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::ServiceBase::WeakPtr & weak_ptr : service_ptrs_) {
+    rclcpp::ServiceBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      service_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::ClientBase::WeakPtr & weak_ptr : client_ptrs_) {
+    rclcpp::ClientBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      client_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::TimerBase::WeakPtr & weak_ptr : timer_ptrs_) {
+    rclcpp::TimerBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      timer_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::Waitable::WeakPtr & weak_ptr : waitable_ptrs_) {
+    rclcpp::Waitable::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      waitable_func(ref_ptr);
+    }
+  }
+}
+
 std::atomic_bool &
 CallbackGroup::get_associated_with_executor_atomic()
 {
@@ -52,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
+rclcpp::GuardCondition::SharedPtr
+CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
+    if (associated_with_executor_) {
+      trigger_notify_guard_condition();
+    }
+    notify_guard_condition_ = nullptr;
+  }
+
+  if (!notify_guard_condition_) {
+    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
+  }
+
+  return notify_guard_condition_;
+}
+
+void
+CallbackGroup::trigger_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_) {
+    notify_guard_condition_->trigger();
+  }
+}
+
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/client.cpp

@@ -38,7 +38,8 @@ ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
 : node_graph_(node_graph),
   node_handle_(node_base->get_shared_rcl_node_handle()),
-  context_(node_base->get_context())
+  context_(node_base->get_context()),
+  node_logger_(rclcpp::get_node_logger(node_handle_.get()))
 {
   std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
   rcl_client_t * new_rcl_client = new rcl_client_t;
@@ -66,6 +67,7 @@ ClientBase::ClientBase(
 
 ClientBase::~ClientBase()
 {
+  clear_on_new_response_callback();
   // Make sure the client handle is destructed as early as possible and before the node handle
   client_handle_.reset();
 }
@@ -198,3 +200,55 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+rclcpp::QoS
+ClientBase::get_request_publisher_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_client_request_publisher_get_actual_qos(client_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get client's request publisher qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS request_publisher_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return request_publisher_qos;
+}
+
+rclcpp::QoS
+ClientBase::get_response_subscription_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_client_response_subscription_get_actual_qos(client_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get client's response subscription qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS response_subscription_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return response_subscription_qos;
+}
+
+void
+ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data)
+{
+  rcl_ret_t ret = rcl_client_set_on_new_response_callback(
+    client_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(ret, "failed to set the on new response callback for client");
+  }
+}

rclcpp/src/rclcpp/clock.cpp

@@ -105,13 +105,17 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
     });
 
   if (this_clock_type == RCL_STEADY_TIME) {
-    auto steady_time = std::chrono::steady_clock::time_point(
-      std::chrono::nanoseconds(until.nanoseconds()));
+    // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch
+    const Time rcl_entry = now();
+    const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now();
+    const Duration delta_t = until - rcl_entry;
+    const std::chrono::steady_clock::time_point chrono_until =
+      chrono_entry + std::chrono::nanoseconds(delta_t.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);
+      cv.wait_until(lock, chrono_until);
     }
   } else if (this_clock_type == RCL_SYSTEM_TIME) {
     auto system_time = std::chrono::system_clock::time_point(
@@ -172,6 +176,12 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
   return now() >= until;
 }
 
+bool
+Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
+{
+  return sleep_until(now() + rel_time, context);
+}
+
 bool
 Clock::ros_time_is_active()
 {

rclcpp/src/rclcpp/context.cpp

@@ -32,8 +32,6 @@
 #include "rcutils/error_handling.h"
 #include "rcutils/macros.h"
 
-#include "rmw/impl/cpp/demangle.hpp"
-
 #include "./logging_mutex.hpp"
 
 using rclcpp::Context;
@@ -192,7 +190,7 @@ __delete_context(rcl_context_t * context)
 void
 Context::init(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const rclcpp::InitOptions & init_options)
 {
   std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
@@ -219,7 +217,7 @@ Context::init(
     if (0u == count) {
       ret = rcl_logging_configure_with_output_handler(
         &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
+        rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
         rclcpp_logging_output_handler);
       if (RCL_RET_OK != ret) {
         rcl_context_.reset();

rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp

@@ -0,0 +1,39 @@
+// 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/add_guard_condition_to_rcl_wait_set.hpp"
+#include "rclcpp/exceptions.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+void
+add_guard_condition_to_rcl_wait_set(
+  rcl_wait_set_t & wait_set,
+  const rclcpp::GuardCondition & guard_condition)
+{
+  const auto & gc = guard_condition.get_rcl_guard_condition();
+
+  rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &gc, NULL);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to add guard condition to wait set");
+  }
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "./resolve_parameter_overrides.hpp"
+#include "resolve_parameter_overrides.hpp"
 
 #include <string>
 #include <map>
@@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides(
         [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/detail/utilities.cpp

@@ -31,7 +31,7 @@ namespace detail
 
 std::vector<std::string>
 get_unparsed_ros_arguments(
-  int argc, char const * const argv[],
+  int argc, char const * const * argv,
   rcl_arguments_t * arguments,
   rcl_allocator_t allocator)
 {

rclcpp/src/rclcpp/duration.cpp

@@ -37,11 +37,6 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
   rcl_duration_.nanoseconds += nanoseconds;
 }
 
-Duration::Duration(rcl_duration_value_t nanoseconds)
-{
-  rcl_duration_.nanoseconds = nanoseconds;
-}
-
 Duration::Duration(std::chrono::nanoseconds nanoseconds)
 {
   rcl_duration_.nanoseconds = nanoseconds.count();

rclcpp/src/rclcpp/executor.cpp

@@ -45,19 +45,13 @@ using rclcpp::FutureReturnCode;
 
 Executor::Executor(const rclcpp::ExecutorOptions & options)
 : spinning(false),
+  interrupt_guard_condition_(options.context),
   shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   memory_strategy_(options.memory_strategy)
 {
   // Store the context for later use.
   context_ = options.context;
 
-  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  rcl_ret_t ret = rcl_guard_condition_init(
-    &interrupt_guard_condition_, context_->get_rcl_context().get(), guard_condition_options);
-  if (RCL_RET_OK != ret) {
-    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
-  }
-
   shutdown_callback_handle_ = context_->add_on_shutdown_callback(
     [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {
       auto strong_gc = weak_gc.lock();
@@ -68,13 +62,13 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
 
   // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
   // and one for the executor's guard cond (interrupt_guard_condition_)
-  memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
+  memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get());
 
   // Put the executor's guard condition in
-  memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
+  memory_strategy_->add_guard_condition(interrupt_guard_condition_);
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
-  ret = rcl_wait_set_init(
+  rcl_ret_t ret = rcl_wait_set_init(
     &wait_set_,
     0, 2, 0, 0, 0, 0,
     context_->get_rcl_context().get(),
@@ -84,12 +78,6 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
       "rclcpp",
       "failed to create wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
-    if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string().str);
-      rcl_reset_error();
-    }
     throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
@@ -118,11 +106,11 @@ Executor::~Executor()
   weak_groups_associated_with_executor_to_nodes_.clear();
   weak_groups_to_nodes_associated_with_executor_.clear();
   weak_groups_to_nodes_.clear();
-  for (const auto & pair : weak_nodes_to_guard_conditions_) {
-    auto & guard_condition = pair.second;
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
+    auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  weak_nodes_to_guard_conditions_.clear();
+  weak_groups_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -131,15 +119,9 @@ Executor::~Executor()
       "failed to destroy wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
-  // Finalize the interrupt guard condition.
-  if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "failed to destroy guard condition: %s", rcl_get_error_string().str);
-    rcl_reset_error();
-  }
   // Remove and release the sigint guard condition
-  memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
+  memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get());
+  memory_strategy_->remove_guard_condition(&interrupt_guard_condition_);
 
   // Remove shutdown callback handle registered to Context
   if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
@@ -222,8 +204,7 @@ Executor::add_callback_group_to_map(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Callback group has already been added to an executor.");
   }
-  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info =
     weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -233,18 +214,24 @@ Executor::add_callback_group_to_map(
   }
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-  if (is_new_node) {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
-    if (notify) {
-      // Interrupt waiting to handle new node
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
-      }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
+    // Add the callback_group's notify condition to the guard condition handles
+    memory_strategy_->add_guard_condition(*callback_group_guard_condition);
+  }
+
+  if (notify) {
+    // Interrupt waiting to handle new node
+    try {
+      interrupt_guard_condition_.trigger();
+    } catch (const rclcpp::exceptions::RCLError & ex) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to trigger guard condition on callback group add: ") + ex.what());
     }
-    // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -315,15 +302,21 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (iter != weak_groups_to_guard_conditions_.end()) {
+      memory_strategy_->remove_guard_condition(iter->second);
+    }
+    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
     if (notify) {
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
+      try {
+        interrupt_guard_condition_.trigger();
+      } catch (const rclcpp::exceptions::RCLError & ex) {
+        throw std::runtime_error(
+                std::string(
+                  "Failed to trigger guard condition on callback group remove: ") + ex.what());
       }
     }
-    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -427,8 +420,8 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration)
 
 void Executor::spin_all(std::chrono::nanoseconds max_duration)
 {
-  if (max_duration <= 0ns) {
-    throw std::invalid_argument("max_duration must be positive");
+  if (max_duration < 0ns) {
+    throw std::invalid_argument("max_duration must be greater than or equal to 0");
   }
   return this->spin_some_impl(max_duration, true);
 }
@@ -494,9 +487,11 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
+  try {
+    interrupt_guard_condition_.trigger();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("Failed to trigger guard condition in cancel: ") + ex.what());
   }
 }
 
@@ -541,9 +536,12 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
+  try {
+    interrupt_guard_condition_.trigger();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to trigger guard condition from execute_any_executable: ") + ex.what());
   }
 }
 
@@ -708,12 +706,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         auto weak_node_ptr = pair.second;
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
-          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
-            auto guard_condition = node_guard_pair->second;
-            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-            memory_strategy_->remove_guard_condition(guard_condition);
-          }
         }
       }
       std::for_each(
@@ -729,6 +721,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
           {
             weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
           }
+          auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr);
+          if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) {
+            auto guard_condition = callback_guard_pair->second;
+            weak_groups_to_guard_conditions_.erase(group_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
           weak_groups_to_nodes_.erase(group_ptr);
         });
     }
@@ -936,3 +934,9 @@ Executor::has_node(
       return other_ptr == node_ptr;
     }) != weak_groups_to_nodes.end();
 }
+
+bool
+Executor::is_spinning()
+{
+  return spinning;
+}

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -72,8 +72,9 @@ MultiThreadedExecutor::get_number_of_threads()
 }
 
 void
-MultiThreadedExecutor::run(size_t)
+MultiThreadedExecutor::run(size_t this_thread_number)
 {
+  (void)this_thread_number;
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {

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/detail/add_guard_condition_to_rcl_wait_set.hpp"
 
 using rclcpp::executors::StaticExecutorEntitiesCollector;
 
@@ -108,7 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
   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();
+      const auto & gc = node_ptr->get_notify_guard_condition();
+      weak_nodes_to_guard_conditions_[node_ptr] = &gc;
     }
   }
   new_nodes_.clear();
@@ -265,18 +267,14 @@ StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeo
   }
 }
 
-bool
+void
 StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   // Add waitable guard conditions (one for each registered node) into the wait set.
   for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto & gc = pair.second;
-    rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL);
-    if (ret != RCL_RET_OK) {
-      throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set");
-    }
+    detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc);
   }
-  return true;
 }
 
 size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
@@ -441,8 +439,9 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
       auto found_guard_condition = std::find_if(
         weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(),
         [&](std::pair<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-        const rcl_guard_condition_t *> pair) -> bool {
-          return pair.second == p_wait_set->guard_conditions[i];
+        const GuardCondition *> pair) -> bool {
+          const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition();
+          return &rcl_gc == p_wait_set->guard_conditions[i];
         });
       if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) {
         return true;

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -71,8 +71,8 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration)
 void
 StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
 {
-  if (max_duration <= std::chrono::nanoseconds(0)) {
-    throw std::invalid_argument("max_duration must be positive");
+  if (max_duration < std::chrono::nanoseconds(0)) {
+    throw std::invalid_argument("max_duration must be greater than or equal to 0");
   }
   return this->spin_some_impl(max_duration, true);
 }
@@ -139,9 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group(
   bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
   if (is_new_node && notify) {
     // Interrupt waiting to handle new node
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
-    }
+    interrupt_guard_condition_.trigger();
   }
 }
 
@@ -152,9 +150,7 @@ StaticSingleThreadedExecutor::add_node(
   bool is_new_node = entities_collector_->add_node(node_ptr);
   if (is_new_node && notify) {
     // Interrupt waiting to handle new node
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
-    }
+    interrupt_guard_condition_.trigger();
   }
 }
 
@@ -171,9 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group(
   bool node_removed = entities_collector_->remove_callback_group(group_ptr);
   // If the node was matched and removed, interrupt waiting
   if (node_removed && notify) {
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
-    }
+    interrupt_guard_condition_.trigger();
   }
 }
 
@@ -187,9 +181,7 @@ StaticSingleThreadedExecutor::remove_node(
   }
   // If the node was matched and removed, interrupt waiting
   if (notify) {
-    if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string().str);
-    }
+    interrupt_guard_condition_.trigger();
   }
 }
 

rclcpp/src/rclcpp/generic_publisher.cpp

@@ -31,4 +31,49 @@ void GenericPublisher::publish(const rclcpp::SerializedMessage & message)
   }
 }
 
+void GenericPublisher::publish_as_loaned_msg(const rclcpp::SerializedMessage & message)
+{
+  auto loaned_message = borrow_loaned_message();
+  deserialize_message(message.get_rcl_serialized_message(), loaned_message);
+  publish_loaned_message(loaned_message);
+}
+
+void * GenericPublisher::borrow_loaned_message()
+{
+  void * loaned_message = nullptr;
+  auto return_code = rcl_borrow_loaned_message(
+    get_publisher_handle().get(), &type_support_, &loaned_message);
+
+  if (return_code != RMW_RET_OK) {
+    if (return_code == RCL_RET_UNSUPPORTED) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        return_code,
+        "current middleware cannot support loan messages");
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to borrow loaned msg");
+    }
+  }
+  return loaned_message;
+}
+
+void GenericPublisher::deserialize_message(
+  const rmw_serialized_message_t & serialized_message,
+  void * deserialized_msg)
+{
+  auto return_code = rmw_deserialize(&serialized_message, &type_support_, deserialized_msg);
+  if (return_code != RMW_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to deserialize msg");
+  }
+}
+
+void GenericPublisher::publish_loaned_message(void * loaned_message)
+{
+  auto return_code = rcl_publish_loaned_message(
+    get_publisher_handle().get(), loaned_message, NULL);
+
+  if (return_code != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish loaned message");
+  }
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/graph_listener.cpp

@@ -22,6 +22,7 @@
 
 #include "rcl/error_handling.h"
 #include "rcl/types.h"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
@@ -40,19 +41,9 @@ GraphListener::GraphListener(const std::shared_ptr<Context> & parent_context)
 : weak_parent_context_(parent_context),
   rcl_parent_context_(parent_context->get_rcl_context()),
   is_started_(false),
-  is_shutdown_(false)
+  is_shutdown_(false),
+  interrupt_guard_condition_(parent_context)
 {
-  // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked
-  //   automatically with the rcl guard condition
-  // hold on to this context to prevent it from going out of scope while this
-  // guard condition is using it.
-  rcl_ret_t ret = rcl_guard_condition_init(
-    &interrupt_guard_condition_,
-    rcl_parent_context_.get(),
-    rcl_guard_condition_get_default_options());
-  if (RCL_RET_OK != ret) {
-    throw_from_rcl_error(ret, "failed to create interrupt guard condition");
-  }
 }
 
 GraphListener::~GraphListener()
@@ -159,10 +150,7 @@ GraphListener::run_loop()
       throw_from_rcl_error(ret, "failed to clear wait set");
     }
     // Put the interrupt guard condition in the wait set.
-    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL);
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
-    }
+    detail::add_guard_condition_to_rcl_wait_set(wait_set_, interrupt_guard_condition_);
 
     // Put graph guard conditions for each node into the wait set.
     std::vector<size_t> graph_gc_indexes(node_graph_interfaces_size, 0u);
@@ -211,19 +199,16 @@ GraphListener::run_loop()
 }
 
 static void
-interrupt_(rcl_guard_condition_t * interrupt_guard_condition)
+interrupt_(GuardCondition * interrupt_guard_condition)
 {
-  rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition);
-  if (RCL_RET_OK != ret) {
-    throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition");
-  }
+  interrupt_guard_condition->trigger();
 }
 
 static void
 acquire_nodes_lock_(
   std::mutex * node_graph_interfaces_barrier_mutex,
   std::mutex * node_graph_interfaces_mutex,
-  rcl_guard_condition_t * interrupt_guard_condition)
+  GuardCondition * interrupt_guard_condition)
 {
   {
     // Acquire this lock to prevent the run loop from re-locking the
@@ -351,10 +336,6 @@ GraphListener::__shutdown()
       interrupt_(&interrupt_guard_condition_);
       listener_thread_.join();
     }
-    rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
-    }
     if (is_started_) {
       cleanup_wait_set();
     }

rclcpp/src/rclcpp/guard_condition.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "rclcpp/guard_condition.hpp"
 
 #include "rclcpp/exceptions.hpp"
@@ -20,19 +22,20 @@
 namespace rclcpp
 {
 
-GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context)
+GuardCondition::GuardCondition(
+  rclcpp::Context::SharedPtr context,
+  rcl_guard_condition_options_t guard_condition_options)
 : context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()}
 {
   if (!context_) {
     throw std::invalid_argument("context argument unexpectedly nullptr");
   }
-  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
   rcl_ret_t ret = rcl_guard_condition_init(
     &this->rcl_guard_condition_,
     context_->get_rcl_context().get(),
     guard_condition_options);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition");
   }
 }
 
@@ -45,7 +48,7 @@ GuardCondition::~GuardCondition()
     } catch (const std::exception & exception) {
       RCLCPP_ERROR(
         rclcpp::get_logger("rclcpp"),
-        "Error in destruction of rcl guard condition: %s", exception.what());
+        "failed to finalize guard condition: %s", exception.what());
     }
   }
 }
@@ -56,6 +59,12 @@ GuardCondition::get_context() const
   return context_;
 }
 
+rcl_guard_condition_t &
+GuardCondition::get_rcl_guard_condition()
+{
+  return rcl_guard_condition_;
+}
+
 const rcl_guard_condition_t &
 GuardCondition::get_rcl_guard_condition() const
 {
@@ -65,9 +74,16 @@ GuardCondition::get_rcl_guard_condition() const
 void
 GuardCondition::trigger()
 {
-  rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  if (on_trigger_callback_) {
+    on_trigger_callback_(1);
+  } else {
+    rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+    unread_count_++;
   }
 }
 
@@ -77,4 +93,42 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state)
   return in_use_by_wait_set_.exchange(in_use_state);
 }
 
+void
+GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  if (exchange_in_use_by_wait_set_state(true)) {
+    if (wait_set != wait_set_) {
+      throw std::runtime_error("guard condition has already been added to a wait set.");
+    }
+  } else {
+    wait_set_ = wait_set;
+  }
+
+  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to add guard condition to wait set");
+  }
+}
+
+void
+GuardCondition::set_on_trigger_callback(std::function<void(size_t)> callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  if (callback) {
+    on_trigger_callback_ = callback;
+
+    if (unread_count_) {
+      callback(unread_count_);
+      unread_count_ = 0;
+    }
+    return;
+  }
+
+  on_trigger_callback_ = nullptr;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -19,7 +19,7 @@ using rclcpp::memory_strategy::MemoryStrategy;
 
 rclcpp::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
-  std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+  const std::shared_ptr<const rcl_subscription_t> & subscriber_handle,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -40,7 +40,7 @@ MemoryStrategy::get_subscription_by_handle(
 
 rclcpp::ServiceBase::SharedPtr
 MemoryStrategy::get_service_by_handle(
-  std::shared_ptr<const rcl_service_t> service_handle,
+  const std::shared_ptr<const rcl_service_t> & service_handle,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -61,7 +61,7 @@ MemoryStrategy::get_service_by_handle(
 
 rclcpp::ClientBase::SharedPtr
 MemoryStrategy::get_client_by_handle(
-  std::shared_ptr<const rcl_client_t> client_handle,
+  const std::shared_ptr<const rcl_client_t> & client_handle,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -82,7 +82,7 @@ MemoryStrategy::get_client_by_handle(
 
 rclcpp::TimerBase::SharedPtr
 MemoryStrategy::get_timer_by_handle(
-  std::shared_ptr<const rcl_timer_t> timer_handle,
+  const std::shared_ptr<const rcl_timer_t> & timer_handle,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -103,7 +103,7 @@ MemoryStrategy::get_timer_by_handle(
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 MemoryStrategy::get_node_by_group(
-  rclcpp::CallbackGroup::SharedPtr group,
+  const rclcpp::CallbackGroup::SharedPtr & group,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   if (!group) {
@@ -121,7 +121,7 @@ MemoryStrategy::get_node_by_group(
 
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_subscription(
-  rclcpp::SubscriptionBase::SharedPtr subscription,
+  const rclcpp::SubscriptionBase::SharedPtr & subscription,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -143,7 +143,7 @@ MemoryStrategy::get_group_by_subscription(
 
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_service(
-  rclcpp::ServiceBase::SharedPtr service,
+  const rclcpp::ServiceBase::SharedPtr & service,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -165,7 +165,7 @@ MemoryStrategy::get_group_by_service(
 
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_client(
-  rclcpp::ClientBase::SharedPtr client,
+  const rclcpp::ClientBase::SharedPtr & client,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -187,7 +187,7 @@ MemoryStrategy::get_group_by_client(
 
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_timer(
-  rclcpp::TimerBase::SharedPtr timer,
+  const rclcpp::TimerBase::SharedPtr & timer,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
@@ -209,7 +209,7 @@ MemoryStrategy::get_group_by_timer(
 
 rclcpp::CallbackGroup::SharedPtr
 MemoryStrategy::get_group_by_waitable(
-  rclcpp::Waitable::SharedPtr waitable,
+  const rclcpp::Waitable::SharedPtr & waitable,
   const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {

rclcpp/src/rclcpp/node.cpp

@@ -310,24 +310,6 @@ Node::create_callback_group(
   return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }
 
-const rclcpp::ParameterValue &
-Node::declare_parameter(const std::string & name)
-{
-#ifndef _WIN32
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  return this->node_parameters_->declare_parameter(name);
-#ifndef _WIN32
-# pragma GCC diagnostic pop
-#else
-# pragma warning(pop)
-#endif
-}
-
 const rclcpp::ParameterValue &
 Node::declare_parameter(
   const std::string & name,

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,93 +31,6 @@ using rclcpp::exceptions::throw_from_rcl_error;
 
 using rclcpp::node_interfaces::NodeBase;
 
-namespace
-{
-/// A class to manage the lifetime of a node handle and its context
-/**
- * This class bundles together the lifetime of the rcl_node_t handle with the
- * lifetime of the RCL context. This ensures that the context will remain alive
- * for as long as the rcl_node_t handle is alive.
- */
-class NodeHandleWithContext
-{
-public:
-  /// Make an instance of a NodeHandleWithContext
-  /**
-   * The make function returns a std::shared_ptr<rcl_node_t> which is actually
-   * an alias for a std::shared_ptr<NodeHandleWithContext>. There is no use for
-   * accessing a NodeHandleWithContext instance directly, because its only job
-   * is to couple together the lifetime of a context with the lifetime of a
-   * node handle that depends on it.
-   */
-  static std::shared_ptr<rcl_node_t>
-  make(
-    rclcpp::Context::SharedPtr context,
-    std::shared_ptr<std::recursive_mutex> logging_mutex,
-    rcl_node_t * node_handle)
-  {
-    auto aliased_ptr = std::make_shared<NodeHandleWithContext>(
-      NodeHandleWithContext(
-        std::move(context),
-        std::move(logging_mutex),
-        node_handle));
-
-    return std::shared_ptr<rcl_node_t>(std::move(aliased_ptr), node_handle);
-  }
-
-  // This class should not be copied. It should only exist in the
-  // std::shared_ptr that it was originally provided in.
-  NodeHandleWithContext(const NodeHandleWithContext &) = delete;
-  NodeHandleWithContext & operator=(const NodeHandleWithContext &) = delete;
-  NodeHandleWithContext & operator=(NodeHandleWithContext &&) = delete;
-
-  NodeHandleWithContext(NodeHandleWithContext && other)
-  : context_(std::move(other.context_)),
-    logging_mutex_(std::move(other.logging_mutex_)),
-    node_handle_(other.node_handle_)
-  {
-    other.node_handle_ = nullptr;
-  }
-
-  ~NodeHandleWithContext()
-  {
-    if (!node_handle_) {
-      // If the node_handle_ is null, then this object was moved-from. We don't
-      // need to do any cleanup.
-      return;
-    }
-
-    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
-    // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
-    // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
-    // here directly.
-    if (rcl_node_fini(node_handle_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
-    }
-    delete node_handle_;
-  }
-
-private:
-  // The constructor is private because this class is only meant to be aliased
-  // into a std::shared_ptr<rcl_node_t> and should not be constructed any other
-  // way.
-  NodeHandleWithContext(
-    rclcpp::Context::SharedPtr context,
-    std::shared_ptr<std::recursive_mutex> logging_mutex,
-    rcl_node_t * node_handle)
-  : context_(std::move(context)),
-    logging_mutex_(std::move(logging_mutex)),
-    node_handle_(node_handle)
-  {}
-
-  rclcpp::Context::SharedPtr context_;
-  std::shared_ptr<std::recursive_mutex> logging_mutex_;
-  rcl_node_t * node_handle_;
-};
-}  // anonymous namespace
-
 NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
@@ -132,30 +44,15 @@ NodeBase::NodeBase(
   node_handle_(nullptr),
   default_callback_group_(nullptr),
   associated_with_executor_(false),
+  notify_guard_condition_(context),
   notify_guard_condition_is_valid_(false)
 {
-  // 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(
-    &notify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options);
-  if (ret != RCL_RET_OK) {
-    throw_from_rcl_error(ret, "failed to create interrupt guard condition");
-  }
-
-  // Setup a safe exit lamda to clean up the guard condition in case of an error here.
-  auto finalize_notify_guard_condition = [this]() {
-      // Finalize the interrupt guard condition.
-      if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "failed to destroy guard condition: %s", rcl_get_error_string().str);
-      }
-    };
-
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
   std::unique_ptr<rcl_node_t> rcl_node(new rcl_node_t(rcl_get_zero_initialized_node()));
 
   std::shared_ptr<std::recursive_mutex> logging_mutex = get_global_logging_mutex();
+
+  rcl_ret_t ret;
   {
     std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
     // TODO(ivanpauno): /rosout Qos should be reconfigurable.
@@ -168,9 +65,6 @@ NodeBase::NodeBase(
       context_->get_rcl_context().get(), &rcl_node_options);
   }
   if (ret != RCL_RET_OK) {
-    // Finalize the interrupt guard condition.
-    finalize_notify_guard_condition();
-
     if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // discard rcl_node_init error
       int validation_result;
@@ -219,7 +113,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;
@@ -235,11 +142,6 @@ NodeBase::~NodeBase()
   {
     std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
     notify_guard_condition_is_valid_ = false;
-    if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string().str);
-    }
   }
 }
 
@@ -282,13 +184,13 @@ NodeBase::get_rcl_node_handle() const
 std::shared_ptr<rcl_node_t>
 NodeBase::get_shared_rcl_node_handle()
 {
-  return node_handle_;
+  return std::shared_ptr<rcl_node_t>(shared_from_this(), node_handle_.get());
 }
 
 std::shared_ptr<const rcl_node_t>
 NodeBase::get_shared_rcl_node_handle() const
 {
-  return node_handle_;
+  return std::shared_ptr<const rcl_node_t>(shared_from_this(), node_handle_.get());
 }
 
 rclcpp::CallbackGroup::SharedPtr
@@ -340,20 +242,14 @@ NodeBase::get_associated_with_executor_atomic()
   return associated_with_executor_;
 }
 
-rcl_guard_condition_t *
+rclcpp::GuardCondition &
 NodeBase::get_notify_guard_condition()
 {
   std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
   if (!notify_guard_condition_is_valid_) {
-    return nullptr;
+    throw std::runtime_error("failed to get notify guard condition because it is invalid");
   }
-  return &notify_guard_condition_;
-}
-
-std::unique_lock<std::recursive_mutex>
-NodeBase::acquire_notify_guard_condition_lock() const
-{
-  return std::unique_lock<std::recursive_mutex>(notify_guard_condition_mutex_);
+  return notify_guard_condition_;
 }
 
 bool

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -533,12 +533,12 @@ NodeGraph::notify_graph_change()
     }
   }
   graph_cv_.notify_all();
-  {
-    auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition());
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to trigger notify guard condition");
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on graph change: ") + ex.what());
   }
 }
 

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -236,6 +236,9 @@ __check_parameters(
     } else {
       descriptor = parameter_infos[name].descriptor;
     }
+    if (descriptor.name.empty()) {
+      descriptor.name = name;
+    }
     const auto new_type = parameter.get_type();
     const auto specified_type = static_cast<rclcpp::ParameterType>(descriptor.type);
     result.successful = descriptor.dynamic_typing || specified_type == new_type;
@@ -464,14 +467,6 @@ declare_parameter_helper(
   return parameters.at(name).value;
 }
 
-const rclcpp::ParameterValue &
-NodeParameters::declare_parameter(const std::string & name)
-{
-  rcl_interfaces::msg::ParameterDescriptor descriptor;
-  descriptor.dynamic_typing = true;
-  return this->declare_parameter(name, rclcpp::ParameterValue{}, descriptor, false);
-}
-
 const rclcpp::ParameterValue &
 NodeParameters::declare_parameter(
   const std::string & name,

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -35,20 +35,20 @@ NodeServices::add_service(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create service, group not in node.");
     }
-    group->add_service(service_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_service(service_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_service(service_base_ptr);
+
   // Notify the executor that a new service was created using the parent Node.
-  {
-    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
-      throw std::runtime_error(
-              std::string("Failed to notify wait set on service creation: ") +
-              rmw_get_error_string().str
-      );
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on service creation: ") + ex.what());
   }
 }
 
@@ -62,20 +62,20 @@ NodeServices::add_client(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create client, group not in node.");
     }
-    group->add_client(client_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_client(client_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_client(client_base_ptr);
+
   // Notify the executor that a new client was created using the parent Node.
-  {
-    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
-      throw std::runtime_error(
-              std::string("Failed to notify wait set on client creation: ") +
-              rmw_get_error_string().str
-      );
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on client creation: ") + ex.what());
   }
 }
 

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -37,15 +37,20 @@ NodeTimers::add_timer(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create timer, group not in node.");
     }
-    callback_group->add_timer(timer);
   } else {
-    node_base_->get_default_callback_group()->add_timer(timer);
+    callback_group = node_base_->get_default_callback_group();
   }
-  if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+  callback_group->add_timer(timer);
+
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
-            std::string("Failed to notify wait set on timer creation: ") +
-            rmw_get_error_string().str);
+            std::string("failed to notify wait set on timer creation: ") + ex.what());
   }
+
   TRACEPOINT(
     rclcpp_timer_link_node,
     static_cast<const void *>(timer->get_timer_handle().get()),

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -64,18 +64,19 @@ NodeTopics::add_publisher(
     callback_group = node_base_->get_default_callback_group();
   }
 
-  for (auto & publisher_event : publisher->get_event_handlers()) {
+  for (auto & key_event_pair : publisher->get_event_handlers()) {
+    auto publisher_event = key_event_pair.second;
     callback_group->add_waitable(publisher_event);
   }
 
   // Notify the executor that a new publisher was created using the parent Node.
-  {
-    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
-      throw std::runtime_error(
-              std::string("Failed to notify wait set on publisher creation: ") +
-              rmw_get_error_string().str);
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on publisher creation: ") + ex.what());
   }
 }
 
@@ -106,7 +107,8 @@ NodeTopics::add_subscription(
 
   callback_group->add_subscription(subscription);
 
-  for (auto & subscription_event : subscription->get_event_handlers()) {
+  for (auto & key_event_pair : subscription->get_event_handlers()) {
+    auto subscription_event = key_event_pair.second;
     callback_group->add_waitable(subscription_event);
   }
 
@@ -117,13 +119,13 @@ NodeTopics::add_subscription(
   }
 
   // Notify the executor that a new subscription was created using the parent Node.
-  {
-    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition());
-    if (ret != RCL_RET_OK) {
-      using rclcpp::exceptions::throw_from_rcl_error;
-      throw_from_rcl_error(ret, "failed to notify wait set on subscription creation");
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on subscription creation: ") + ex.what());
   }
 }
 

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -35,20 +35,20 @@ NodeWaitables::add_waitable(
       // TODO(jacobperron): use custom exception
       throw std::runtime_error("Cannot create waitable, group not in node.");
     }
-    group->add_waitable(waitable_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_waitable(waitable_ptr);
+
   // Notify the executor that a new waitable was created using the parent Node.
-  {
-    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
-      throw std::runtime_error(
-              std::string("Failed to notify wait set on waitable creation: ") +
-              rmw_get_error_string().str
-      );
-    }
+  auto & node_gc = node_base_->get_notify_guard_condition();
+  try {
+    node_gc.trigger();
+    group->trigger_notify_guard_condition();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on waitable creation: ") + ex.what());
   }
 }
 

rclcpp/src/rclcpp/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/publisher_base.cpp

@@ -22,6 +22,7 @@
 #include <mutex>
 #include <sstream>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rcutils/logging_macros.h"
@@ -46,7 +47,9 @@ PublisherBase::PublisherBase(
   const rosidl_message_type_support_t & type_support,
   const rcl_publisher_options_t & publisher_options)
 : rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
-  intra_process_is_enabled_(false), intra_process_publisher_id_(0)
+  intra_process_is_enabled_(false),
+  intra_process_publisher_id_(0),
+  type_support_(type_support)
 {
   auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub)
     {
@@ -99,6 +102,11 @@ PublisherBase::PublisherBase(
 
 PublisherBase::~PublisherBase()
 {
+  for (const auto & pair : event_handlers_) {
+    rcl_publisher_event_type_t event_type = pair.first;
+    clear_on_new_qos_event_callback(event_type);
+  }
+
   // must fini the events before fini-ing the publisher
   event_handlers_.clear();
 
@@ -154,7 +162,8 @@ PublisherBase::get_publisher_handle() const
   return publisher_handle_;
 }
 
-const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+const
+std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
 PublisherBase::get_event_handlers() const
 {
   return event_handlers_;

rclcpp/src/rclcpp/qos_event.cpp

@@ -35,6 +35,10 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
 
 QOSEventHandlerBase::~QOSEventHandlerBase()
 {
+  if (on_new_event_callback_) {
+    clear_on_ready_callback();
+  }
+
   if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
@@ -51,14 +55,13 @@ QOSEventHandlerBase::get_number_of_ready_events()
 }
 
 /// Add the Waitable to a wait set.
-bool
+void
 QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_);
   if (RCL_RET_OK != ret) {
     exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set");
   }
-  return true;
 }
 
 /// Check if the Waitable is ready.
@@ -68,4 +71,20 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
   return wait_set->events[wait_set_event_index_] == &event_handle_;
 }
 
+void
+QOSEventHandlerBase::set_on_new_event_callback(
+  rcl_event_callback_t callback,
+  const void * user_data)
+{
+  rcl_ret_t ret = rcl_event_set_callback(
+    &event_handle_,
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(ret, "failed to set the on new message callback for QOS Event");
+  }
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/service.cpp

@@ -28,11 +28,14 @@
 using rclcpp::ServiceBase;
 
 ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
-: node_handle_(node_handle)
+: node_handle_(node_handle),
+  node_logger_(rclcpp::get_node_logger(node_handle_.get()))
 {}
 
 ServiceBase::~ServiceBase()
-{}
+{
+  clear_on_new_request_callback();
+}
 
 bool
 ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)
@@ -84,3 +87,55 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+rclcpp::QoS
+ServiceBase::get_response_publisher_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_service_response_publisher_get_actual_qos(service_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get service's response publisher qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS response_publisher_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return response_publisher_qos;
+}
+
+rclcpp::QoS
+ServiceBase::get_request_subscription_actual_qos() const
+{
+  const rmw_qos_profile_t * qos =
+    rcl_service_request_subscription_get_actual_qos(service_handle_.get());
+  if (!qos) {
+    auto msg =
+      std::string("failed to get service's request subscription qos settings: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  rclcpp::QoS request_subscription_qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+
+  return request_subscription_qos;
+}
+
+void
+ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data)
+{
+  rcl_ret_t ret = rcl_service_set_on_new_request_callback(
+    service_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(ret, "failed to set the on new request callback for service");
+  }
+}

rclcpp/src/rclcpp/signal_handler.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "./signal_handler.hpp"
+#include "signal_handler.hpp"
 
 #include <atomic>
 #include <csignal>

rclcpp/src/rclcpp/subscription_base.cpp

@@ -17,8 +17,11 @@
 #include <cstdio>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
@@ -39,6 +42,7 @@ SubscriptionBase::SubscriptionBase(
   bool is_serialized)
 : node_base_(node_base),
   node_handle_(node_base_->get_shared_rcl_node_handle()),
+  node_logger_(rclcpp::get_node_logger(node_handle_.get())),
   use_intra_process_(false),
   intra_process_subscription_id_(0),
   type_support_(type_support_handle),
@@ -83,6 +87,13 @@ SubscriptionBase::SubscriptionBase(
 
 SubscriptionBase::~SubscriptionBase()
 {
+  clear_on_new_message_callback();
+
+  for (const auto & pair : event_handlers_) {
+    rcl_subscription_event_type_t event_type = pair.first;
+    clear_on_new_qos_event_callback(event_type);
+  }
+
   if (!use_intra_process_) {
     return;
   }
@@ -115,7 +126,8 @@ SubscriptionBase::get_subscription_handle() const
   return subscription_handle_;
 }
 
-const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+const
+std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
 SubscriptionBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -282,7 +294,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state(
   if (get_intra_process_waitable().get() == pointer_to_subscription_part) {
     return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state);
   }
-  for (const auto & qos_event : event_handlers_) {
+  for (const auto & key_event_pair : event_handlers_) {
+    auto qos_event = key_event_pair.second;
     if (qos_event.get() == pointer_to_subscription_part) {
       return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state);
     }
@@ -290,7 +303,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state(
   throw std::runtime_error("given pointer_to_subscription_part does not match any part");
 }
 
-std::vector<rclcpp::NetworkFlowEndpoint> SubscriptionBase::get_network_flow_endpoints() const
+std::vector<rclcpp::NetworkFlowEndpoint>
+SubscriptionBase::get_network_flow_endpoints() const
 {
   rcutils_allocator_t allocator = rcutils_get_default_allocator();
   rcl_network_flow_endpoint_array_t network_flow_endpoint_array =
@@ -326,3 +340,107 @@ std::vector<rclcpp::NetworkFlowEndpoint> SubscriptionBase::get_network_flow_endp
 
   return network_flow_endpoint_vector;
 }
+
+void
+SubscriptionBase::set_on_new_message_callback(
+  rcl_event_callback_t callback,
+  const void * user_data)
+{
+  rcl_ret_t ret = rcl_subscription_set_on_new_message_callback(
+    subscription_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    using rclcpp::exceptions::throw_from_rcl_error;
+    throw_from_rcl_error(ret, "failed to set the on new message callback for subscription");
+  }
+}
+
+bool
+SubscriptionBase::is_cft_enabled() const
+{
+  return rcl_subscription_is_cft_enabled(subscription_handle_.get());
+}
+
+void
+SubscriptionBase::set_content_filter(
+  const std::string & filter_expression,
+  const std::vector<std::string> & expression_parameters)
+{
+  rcl_subscription_content_filter_options_t options =
+    rcl_get_zero_initialized_subscription_content_filter_options();
+
+  std::vector<const char *> cstrings =
+    get_c_vector_string(expression_parameters);
+  rcl_ret_t ret = rcl_subscription_content_filter_options_init(
+    subscription_handle_.get(),
+    get_c_string(filter_expression),
+    cstrings.size(),
+    cstrings.data(),
+    &options);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to init subscription content_filtered_topic option");
+  }
+  RCPPUTILS_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_subscription_content_filter_options_fini(
+      subscription_handle_.get(), &options);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "Failed to fini subscription content_filtered_topic option: %s",
+        rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  });
+
+  ret = rcl_subscription_set_content_filter(
+    subscription_handle_.get(),
+    &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters");
+  }
+}
+
+rclcpp::ContentFilterOptions
+SubscriptionBase::get_content_filter() const
+{
+  rclcpp::ContentFilterOptions ret_options;
+  rcl_subscription_content_filter_options_t options =
+    rcl_get_zero_initialized_subscription_content_filter_options();
+
+  rcl_ret_t ret = rcl_subscription_get_content_filter(
+    subscription_handle_.get(),
+    &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters");
+  }
+
+  RCPPUTILS_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_subscription_content_filter_options_fini(
+      subscription_handle_.get(), &options);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "Failed to fini subscription content_filtered_topic option: %s",
+        rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  });
+
+  rmw_subscription_content_filter_options_t & content_filter_options =
+    options.rmw_subscription_content_filter_options;
+  ret_options.filter_expression = content_filter_options.filter_expression;
+
+  for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) {
+    ret_options.expression_parameters.push_back(
+      content_filter_options.expression_parameters.data[i]);
+  }
+
+  return ret_options;
+}

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -13,16 +13,19 @@
 // limitations under the License.
 
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
 
 using rclcpp::experimental::SubscriptionIntraProcessBase;
 
-bool
+SubscriptionIntraProcessBase::~SubscriptionIntraProcessBase()
+{
+  clear_on_ready_callback();
+}
+
+void
 SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
-
-  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
-  return RCL_RET_OK == ret;
+  detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_);
 }
 
 const char *

rclcpp/src/rclcpp/time_source.cpp

@@ -33,7 +33,7 @@
 namespace rclcpp
 {
 
-class TimeSource::ClocksState : public std::enable_shared_from_this<ClocksState>
+class ClocksState final
 {
 public:
   ClocksState()
@@ -182,12 +182,11 @@ private:
   std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
 };
 
-class TimeSource::NodeState : public std::enable_shared_from_this<NodeState>
+class TimeSource::NodeState final
 {
 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),
+  NodeState(const rclcpp::QoS & qos, bool use_clock_thread)
+  : use_clock_thread_(use_clock_thread),
     logger_(rclcpp::get_logger("rclcpp")),
     qos_(qos)
   {
@@ -250,52 +249,31 @@ public:
     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());
+        rclcpp::ParameterValue(false));
     } 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();
-        }
+        clocks_state_.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());
+      throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'");
     }
-    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);
+      [this](std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event) {
+        if (node_base_ != nullptr) {
+          this->on_parameter_event(event);
         }
-        // Do nothing if the pointer could not be locked because it means the TimeSource is now
+        // Do nothing if node_base_ is nullptr because it means the TimeSource is now
         // without an attached node
       });
   }
@@ -303,12 +281,10 @@ public:
   // 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() *must* be first here, to ensure that the executor
+    // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
+    clocks_state_.disable_ros_time();
     parameter_subscription_.reset();
     node_base_.reset();
     node_topics_.reset();
@@ -316,28 +292,34 @@ public:
     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();
   }
 
+  void attachClock(std::shared_ptr<rclcpp::Clock> clock)
+  {
+    clocks_state_.attachClock(std::move(clock));
+  }
+
+  void detachClock(std::shared_ptr<rclcpp::Clock> clock)
+  {
+    clocks_state_.detachClock(std::move(clock));
+  }
+
 private:
-  std::weak_ptr<ClocksState> clocks_state_;
+  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_;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr};
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr};
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr};
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_{nullptr};
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_{nullptr};
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr};
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_{nullptr};
 
   // Store (and update on node attach) logger for logging.
   Logger logger_;
@@ -346,9 +328,7 @@ private:
   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>;
+  using SubscriptionT = rclcpp::Subscription<rosgraph_msgs::msg::Clock>;
   std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
   std::mutex clock_sub_lock_;
   rclcpp::CallbackGroup::SharedPtr clock_callback_group_;
@@ -358,21 +338,15 @@ private:
   // 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();
+    if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) {
+      clocks_state_.enable_ros_time();
     }
     // Cache the last message in case a new clock is attached.
-    clocks_state_ptr->cache_last_msg(msg);
+    clocks_state_.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);
+      clocks_state_.set_all_clocks(time_msg, true);
     }
   }
 
@@ -421,13 +395,12 @@ private:
       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);
+      [this](std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
+        // We are using node_base_ as an indication if there is a node attached.
+        // Only call the clock_cb if that is the case.
+        if (node_base_ != nullptr) {
+          clock_cb(msg);
         }
-        // Do nothing if the pointer could not be locked because it means the TimeSource is now
-        // without an attached node
       },
       options
     );
@@ -447,19 +420,12 @@ private:
   }
 
   // Parameter Event subscription
-  using ParamMessageT = rcl_interfaces::msg::ParameterEvent;
-  using ParamSubscriptionT = rclcpp::Subscription<ParamMessageT, Alloc>;
+  using ParamSubscriptionT = rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>;
   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;
@@ -475,12 +441,12 @@ private:
       }
       if (it.second->value.bool_value) {
         parameter_state_ = SET_TRUE;
-        clocks_state_ptr->enable_ros_time();
+        clocks_state_.enable_ros_time();
         create_clock_sub();
       } else {
         parameter_state_ = SET_FALSE;
-        clocks_state_ptr->disable_ros_time();
         destroy_clock_sub();
+        clocks_state_.disable_ros_time();
       }
     }
     // Handle the case that use_sim_time was deleted.
@@ -488,7 +454,7 @@ private:
       {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.
+      // If the parameter is deleted mark it as unset but don't change state.
       parameter_state_ = UNSET;
     }
   }
@@ -496,20 +462,14 @@ private:
   // 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)
+: TimeSource(qos, use_clock_thread)
 {
-  clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
   attachNode(node);
 }
 
@@ -519,8 +479,7 @@ TimeSource::TimeSource(
 : constructed_use_clock_thread_(use_clock_thread),
   constructed_qos_(qos)
 {
-  clocks_state_ = std::make_shared<ClocksState>();
-  node_state_ = std::make_shared<NodeState>(clocks_state_->weak_from_this(), qos, use_clock_thread);
+  node_state_ = std::make_shared<NodeState>(qos, use_clock_thread);
 }
 
 void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
@@ -559,19 +518,18 @@ void TimeSource::detachNode()
 {
   node_state_.reset();
   node_state_ = std::make_shared<NodeState>(
-    clocks_state_->weak_from_this(),
     constructed_qos_,
     constructed_use_clock_thread_);
 }
 
 void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
 {
-  clocks_state_->attachClock(std::move(clock));
+  node_state_->attachClock(std::move(clock));
 }
 
 void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 {
-  clocks_state_->detachClock(std::move(clock));
+  node_state_->detachClock(std::move(clock));
 }
 
 bool TimeSource::get_use_clock_thread()