15.2.0

This package uses rcl_interfaces directly, and we've been getting away
with the missing dependency because `rcl` has the entirety of
`rcl_interfaces` as part of its link interface even tough it doesn't
need to.

If (when) `rcl` scopes its dependency to only the c generator and drops
the cpp generator from its link interface, this package will fail to
find the cpp symbols at link time. This change remedies that.

Signed-off-by: Scott K Logan <logans@cottsay.net>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,71 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+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,10 +251,12 @@ ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(TEST cppcheck)
   # must set the property after ament_package()
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
+
+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_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

@@ -476,6 +476,22 @@ public:
     }
   }
 
+  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
+  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);
+      ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr);
+      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ptr);
+      return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
+    } else {
+      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.
   void
   dispatch(
@@ -658,7 +674,7 @@ public:
 
   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);
@@ -677,65 +693,89 @@ public:
 
         // conditions for custom type
         if constexpr (is_ta && std::is_same_v<T, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
+          callback(*message);
         } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
+          callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrCallback>||
             std::is_same_v<T, SharedPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrWithInfoCallback>||
             std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          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<T, SharedConstPtrCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(message);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrWithInfoCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(message, message_info);
         }
         // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
-          callback(*message, message_info);
+        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<T, UniquePtrROSMessageCallback>||
           std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(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<T, UniquePtrWithInfoROSMessageCallback>||
           std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(create_ros_unique_ptr_from_ros_shared_ptr_message(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<T, SharedConstPtrROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(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<T, SharedConstPtrWithInfoROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(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]
@@ -764,7 +804,7 @@ public:
 
   void
   dispatch_intra_process(
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> message,
+    std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
     const rclcpp::MessageInfo & message_info)
   {
     TRACEPOINT(callback_start, static_cast<const void *>(this), true);
@@ -778,70 +818,98 @@ public:
     // Dispatch.
     std::visit(
       [&message, &message_info, this](auto && 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<T, ConstRefCallback>) {
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message);
+          callback(*message);
         } else if constexpr (is_ta && std::is_same_v<T, ConstRefWithInfoCallback>) {  // NOLINT
-          auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message);
-          callback(*local_message, message_info);
+          callback(*message, message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrCallback>||
-            std::is_same_v<T, SharedPtrCallback>
-        ))
+            std::is_same_v<T, SharedPtrCallback>))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, UniquePtrWithInfoCallback>||
             std::is_same_v<T, SharedPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(std::move(message), message_info);
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message));
+          callback(std::move(message));
         } else if constexpr (  // NOLINT[readability/braces]
           is_ta && (
             std::is_same_v<T, SharedConstPtrWithInfoCallback>||
             std::is_same_v<T, ConstRefSharedConstPtrWithInfoCallback>
         ))
         {
-          callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info);
+          callback(std::move(message), message_info);
         }
         // conditions for ros message type
-        else if constexpr (std::is_same_v<T, ConstRefROSMessageCallback>) {  // NOLINT
-          callback(*message);
-        } else if constexpr (std::is_same_v<T, ConstRefWithInfoROSMessageCallback>) {
-          callback(*message, message_info);
+        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<T, UniquePtrROSMessageCallback>||
           std::is_same_v<T, SharedPtrROSMessageCallback>)
         {
-          callback(std::move(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<T, UniquePtrWithInfoROSMessageCallback>||
           std::is_same_v<T, SharedPtrWithInfoROSMessageCallback>)
         {
-          callback(std::move(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<T, SharedConstPtrROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrROSMessageCallback>)
         {
-          callback(std::move(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<T, SharedConstPtrWithInfoROSMessageCallback>||
           std::is_same_v<T, ConstRefSharedConstPtrWithInfoROSMessageCallback>)
         {
-          callback(std::move(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]

rclcpp/include/rclcpp/callback_group.hpp

@@ -138,6 +138,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

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>

rclcpp/include/rclcpp/clock.hpp

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

@@ -48,17 +48,20 @@ public:
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;
 
-class OnShutdownCallbackHandle
+class ShutdownCallbackHandle
 {
   friend class Context;
 
 public:
-  using OnShutdownCallbackType = std::function<void ()>;
+  using ShutdownCallbackType = std::function<void ()>;
 
 private:
-  std::weak_ptr<OnShutdownCallbackType> callback;
+  std::weak_ptr<ShutdownCallbackType> callback;
 };
 
+using OnShutdownCallbackHandle = ShutdownCallbackHandle;
+using PreShutdownCallbackHandle = ShutdownCallbackHandle;
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -124,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.
@@ -189,7 +192,7 @@ public:
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;
+  using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -197,7 +200,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronoulsy in the dedicated singal handling thread.
+   * asynchronously in the dedicated singal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -221,7 +224,7 @@ public:
    * to last step in shutdown().
    *
    * When shutdown occurs due to the signal handler, these callbacks are run
-   * asynchronously in the dedicated singal handling thread.
+   * asynchronously in the dedicated signal handling thread.
    *
    * Also, shutdown() may be called from the destructor of this function.
    * Therefore, it is not safe to throw exceptions from these callbacks.
@@ -249,6 +252,33 @@ public:
   bool
   remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
 
+  using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
+
+  /// Add a pre_shutdown callback to be called before shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added.
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated signal handling thread.
+   *
+   * \param[in] callback the pre_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreShutdownCallbackHandle
+  add_pre_shutdown_callback(PreShutdownCallback callback);
+
+  /// Remove an registered pre_shutdown callback.
+  /**
+   * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
+
   /// Return the shutdown callbacks.
   /**
    * Returned callbacks are a copy of the registered callbacks.
@@ -257,6 +287,14 @@ public:
   std::vector<OnShutdownCallback>
   get_on_shutdown_callbacks() const;
 
+  /// Return the pre-shutdown callbacks.
+  /**
+   * Returned callbacks are a copy of the registered callbacks.
+   */
+  RCLCPP_PUBLIC
+  std::vector<PreShutdownCallback>
+  get_pre_shutdown_callbacks() const;
+
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_context_t>
@@ -338,6 +376,9 @@ private:
   std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
+  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::mutex pre_shutdown_callbacks_mutex_;
+
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;
   /// Mutex for protecting the global condition variable.
@@ -345,6 +386,29 @@ private:
 
   /// Keep shared ownership of global vector of weak contexts
   std::shared_ptr<WeakContextsWrapper> weak_contexts_;
+
+  enum class ShutdownType
+  {
+    pre_shutdown,
+    on_shutdown
+  };
+
+  using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
+
+  RCLCPP_LOCAL
+  ShutdownCallbackHandle
+  add_shutdown_callback(
+    ShutdownType shutdown_type,
+    ShutdownCallback callback);
+
+  RCLCPP_LOCAL
+  bool
+  remove_shutdown_callback(
+    ShutdownType shutdown_type,
+    const ShutdownCallbackHandle & callback_handle);
+
+  std::vector<rclcpp::Context::ShutdownCallback>
+  get_shutdown_callback(ShutdownType shutdown_type) const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/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/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_;
 
@@ -549,7 +561,7 @@ protected:
   spin_once_impl(std::chrono::nanoseconds timeout);
 
   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;
 

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

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

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

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

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

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

rclcpp/include/rclcpp/experimental/executable_list.hpp

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -19,18 +19,16 @@
 
 #include <shared_mutex>
 
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
+#include <iterator>
 #include <memory>
-#include <string>
+#include <stdexcept>
 #include <unordered_map>
 #include <utility>
 #include <vector>
+#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"
@@ -38,6 +36,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/publisher_base.hpp"
+#include "rclcpp/type_adapter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -173,11 +172,14 @@ 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 MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    typename ROSMessageType,
+    typename Alloc,
+    typename Deleter = std::default_delete<MessageT>
+  >
   void
   do_intra_process_publish(
     uint64_t intra_process_publisher_id,
@@ -203,7 +205,7 @@ public:
       // None of the buffers require ownership, so we promote the pointer
       std::shared_ptr<MessageT> msg = std::move(message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         msg, sub_ids.take_shared_subscriptions);
     } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
       sub_ids.take_shared_subscriptions.size() <= 1)
@@ -217,8 +219,7 @@ public:
         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>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message),
         concatenated_vector,
         allocator);
@@ -229,17 +230,19 @@ public:
       // for the buffers that do not require ownership
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+      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>(
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
         std::move(message), sub_ids.take_ownership_subscriptions, allocator);
     }
   }
 
   template<
     typename MessageT,
-    typename Alloc = std::allocator<void>,
-    typename Deleter = std::default_delete<MessageT>>
+    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,
@@ -265,7 +268,7 @@ public:
       // If there are no owning, just convert to shared.
       std::shared_ptr<MessageT> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
@@ -275,17 +278,16 @@ public:
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(allocator, *message);
 
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           shared_msg,
           sub_ids.take_shared_subscriptions);
       }
       if (!sub_ids.take_ownership_subscriptions.empty()) {
-        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
           std::move(message),
           sub_ids.take_ownership_subscriptions,
           allocator);
       }
-
       return shared_msg;
     }
   }
@@ -338,41 +340,83 @@ private:
   template<
     typename MessageT,
     typename Alloc,
-    typename Deleter>
+    typename Deleter,
+    typename ROSMessageType>
   void
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
     std::vector<uint64_t> subscription_ids)
   {
+    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 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");
-        }
-
-        subscription->provide_intra_process_message(message);
-      } 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 Alloc,
+    typename Deleter,
+    typename ROSMessageType>
   void
   add_owned_msg_to_buffers(
     std::unique_ptr<MessageT, Deleter> message,
@@ -382,39 +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)));
         }
+
+        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) {
+        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

@@ -15,25 +15,22 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
 
-#include <rmw/rmw.h>
+#include <rmw/types.h>
 
-#include <functional>
-#include <map>
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <type_traits>
 #include <utility>
 
-#include "rcl/error_handling.h"
+#include "rcl/types.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
-#include "rclcpp/experimental/create_intra_process_buffer.hpp"
-#include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
 
 namespace rclcpp
@@ -43,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,
@@ -98,7 +102,7 @@ public:
   virtual ~SubscriptionIntraProcess() = default;
 
   std::shared_ptr<void>
-  take_data()
+  take_data() override
   {
     ConstMessageSharedPtr shared_msg;
     MessageUniquePtr unique_msg;
@@ -115,9 +119,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:
@@ -154,7 +158,7 @@ protected:
     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,18 +15,17 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <string>
-#include <utility>
 
-#include "rcl/error_handling.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
@@ -39,34 +38,41 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
+  enum class EntityType
+  {
+    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;
+
+  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
@@ -77,14 +83,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

@@ -15,25 +15,21 @@
 #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_
 
-#include <rmw/rmw.h>
-
-#include <functional>
-#include <map>
 #include <memory>
-#include <stdexcept>
 #include <string>
+#include <stdexcept>
 #include <utility>
 
 #include "rcl/error_handling.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-#include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/waitable.hpp"
-#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -41,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(
@@ -67,74 +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_);
-    (void)ret;
+    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

@@ -60,7 +60,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`.

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

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

rclcpp/include/rclcpp/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

@@ -720,6 +720,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_time_source.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

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

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

@@ -20,12 +20,14 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <type_traits>
 #include <utility>
 
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rosidl_runtime_cpp/traits.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
@@ -81,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>;
@@ -263,10 +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(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(std::move(msg));
+      this->do_intra_process_ros_message_publish(std::move(msg));
     }
   }
 
@@ -319,13 +321,28 @@ public:
   >
   publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    // Avoid allocating when not using intra process.
+    if (!intra_process_is_enabled_) {
+      // 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);
+    }
+
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+
+    if (inter_process_publish_needed) {
+      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(std::move(msg));
+    }
   }
 
   /// Publish a message on the topic.
@@ -346,12 +363,7 @@ public:
   >
   publish(const T & msg)
   {
-    // TODO(wjwwood): later update this to give the unique_ptr to the intra
-    // process manager and let it decide if it needs to be converted or not.
-    // For now, convert it unconditionally and pass it the ROSMessageType
-    // publish function specialization.
-
-    // Avoid allocating when not using intra process.
+    // Avoid double allocating when not using intra process.
     if (!intra_process_is_enabled_) {
       // Convert to the ROS message equivalent and publish it.
       ROSMessageType ros_msg;
@@ -359,13 +371,12 @@ public:
       // In this case we're not using intra process.
       return this->do_inter_process_publish(ros_msg);
     }
-    // Otherwise we have to allocate memory in a unique_ptr, convert it,
-    // and pass it along.
+
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     // As the message is not const, a copy should be made.
     // A shared_ptr<const MessageT> could also be constructed here.
-    auto unique_ros_msg = this->create_ros_message_unique_ptr();
-    rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *unique_ros_msg);
-    this->publish(std::move(unique_ros_msg));
+    auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg);
+    this->publish(std::move(unique_msg));
   }
 
   void
@@ -491,7 +502,7 @@ protected:
   }
 
   void
-  do_intra_process_publish(std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
+  do_intra_process_publish(std::unique_ptr<PublishedType, PublishedTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -502,14 +513,32 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    ipm->template do_intra_process_publish<ROSMessageType, AllocatorT>(
+    ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      published_type_allocator_);
+  }
+
+  void
+  do_intra_process_ros_message_publish(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");
+    }
+
+    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_publish_and_return_shared(
+  do_intra_process_ros_message_publish_and_return_shared(
     std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
@@ -521,13 +550,14 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
-    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType,
+    return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
              AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
       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()
@@ -546,6 +576,15 @@ protected:
     return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
   }
 
+  /// Duplicate a given type adapted message as a unique_ptr.
+  std::unique_ptr<PublishedType, PublishedTypeDeleter>
+  duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg)
+  {
+    auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1);
+    PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg);
+    return std::unique_ptr<PublishedType, PublishedTypeDeleter>(ptr, published_type_deleter_);
+  }
+
   /// Copy of original options passed during construction.
   /**
    * It is important to save a copy of this so that the rmw payload which it

rclcpp/include/rclcpp/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>;

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

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 &)
@@ -404,13 +412,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"
@@ -99,9 +103,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 +288,209 @@ 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();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -297,7 +505,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 +515,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 +544,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_factory.hpp

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

rclcpp/include/rclcpp/time_source.hpp

@@ -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.
 /**

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

rclcpp/resource/logging.hpp.em

@@ -141,19 +141,19 @@ 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]}@
 @[ if params]@
 @(''.join(['      ' + p + ', \\\n' for p in params if p != stream_arg]))@
 @[ end if]@
-      logger.get_name(), \
+      (logger).get_name(), \
 @[ if 'stream' not in feature_combination]@
       __VA_ARGS__); \
 @[ else]@
-      "%s", ss.str().c_str()); \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
 @[ end if]@
   } while (0)
 

rclcpp/src/rclcpp/callback_group.cpp

@@ -40,6 +40,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()
 {

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

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

@@ -19,6 +19,7 @@
 #include <sstream>
 #include <string>
 #include <vector>
+#include <unordered_set>
 #include <utility>
 
 #include "rcl/init.h"
@@ -31,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;
@@ -191,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_);
@@ -307,6 +306,15 @@ Context::shutdown(const std::string & reason)
     // if it is not valid, then it cannot be shutdown
     return false;
   }
+
+  // call each pre-shutdown callback
+  {
+    std::lock_guard<std::mutex> lock{pre_shutdown_callbacks_mutex_};
+    for (const auto & callback : pre_shutdown_callbacks_) {
+      (*callback)();
+    }
+  }
+
   // rcl shutdown
   rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
   if (RCL_RET_OK != ret) {
@@ -355,36 +363,118 @@ Context::on_shutdown(OnShutdownCallback callback)
 rclcpp::OnShutdownCallbackHandle
 Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  auto callback_shared_ptr = std::make_shared<OnShutdownCallback>(callback);
-  {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    on_shutdown_callbacks_.emplace(callback_shared_ptr);
-  }
-
-  OnShutdownCallbackHandle callback_handle;
-  callback_handle.callback = callback_shared_ptr;
-  return callback_handle;
+  return add_shutdown_callback(ShutdownType::on_shutdown, callback);
 }
 
 bool
 Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+  return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle);
+}
+
+rclcpp::PreShutdownCallbackHandle
+Context::add_pre_shutdown_callback(PreShutdownCallback callback)
+{
+  return add_shutdown_callback(ShutdownType::pre_shutdown, callback);
+}
+
+bool
+Context::remove_pre_shutdown_callback(
+  const PreShutdownCallbackHandle & callback_handle)
+{
+  return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
+}
+
+rclcpp::ShutdownCallbackHandle
+Context::add_shutdown_callback(
+  ShutdownType shutdown_type,
+  ShutdownCallback callback)
+{
+  auto callback_shared_ptr =
+    std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      {
+        std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
+        pre_shutdown_callbacks_.emplace(callback_shared_ptr);
+      }
+      break;
+    case ShutdownType::on_shutdown:
+      {
+        std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+        on_shutdown_callbacks_.emplace(callback_shared_ptr);
+      }
+      break;
+  }
+
+  ShutdownCallbackHandle callback_handle;
+  callback_handle.callback = callback_shared_ptr;
+  return callback_handle;
+}
+
+bool
+Context::remove_shutdown_callback(
+  ShutdownType shutdown_type,
+  const ShutdownCallbackHandle & callback_handle)
+{
+  std::mutex * mutex_ptr = nullptr;
+  std::unordered_set<
+    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      mutex_ptr = &pre_shutdown_callbacks_mutex_;
+      callback_list_ptr = &pre_shutdown_callbacks_;
+      break;
+    case ShutdownType::on_shutdown:
+      mutex_ptr = &on_shutdown_callbacks_mutex_;
+      callback_list_ptr = &on_shutdown_callbacks_;
+      break;
+  }
+
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
   auto callback_shared_ptr = callback_handle.callback.lock();
   if (callback_shared_ptr == nullptr) {
     return false;
   }
-  return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1;
+  return callback_list_ptr->erase(callback_shared_ptr) == 1;
 }
 
 std::vector<rclcpp::Context::OnShutdownCallback>
 Context::get_on_shutdown_callbacks() const
 {
-  std::vector<OnShutdownCallback> callbacks;
+  return get_shutdown_callback(ShutdownType::on_shutdown);
+}
 
+std::vector<rclcpp::Context::PreShutdownCallback>
+Context::get_pre_shutdown_callbacks() const
+{
+  return get_shutdown_callback(ShutdownType::pre_shutdown);
+}
+
+std::vector<rclcpp::Context::ShutdownCallback>
+Context::get_shutdown_callback(ShutdownType shutdown_type) const
+{
+  std::mutex * mutex_ptr = nullptr;
+  const std::unordered_set<
+    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      mutex_ptr = &pre_shutdown_callbacks_mutex_;
+      callback_list_ptr = &pre_shutdown_callbacks_;
+      break;
+    case ShutdownType::on_shutdown:
+      mutex_ptr = &on_shutdown_callbacks_mutex_;
+      callback_list_ptr = &on_shutdown_callbacks_;
+      break;
+  }
+
+  std::vector<rclcpp::Context::ShutdownCallback> callbacks;
   {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    for (auto & iter : on_shutdown_callbacks_) {
+    std::lock_guard<std::mutex> lock(*mutex_ptr);
+    for (auto & iter : *callback_list_ptr) {
       callbacks.emplace_back(*iter);
     }
   }

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>

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/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");
   }
 }
@@ -119,7 +107,7 @@ Executor::~Executor()
   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;
+    auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
   weak_nodes_to_guard_conditions_.clear();
@@ -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_)) {
@@ -234,17 +216,20 @@ 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();
+    const auto & gc = node_ptr->get_notify_guard_condition();
+    weak_nodes_to_guard_conditions_[node_ptr] = &gc;
     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");
+      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());
+    memory_strategy_->add_guard_condition(gc);
   }
 }
 
@@ -315,15 +300,17 @@ 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);
+    weak_nodes_to_guard_conditions_.erase(node_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());
+    memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -427,8 +414,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 +481,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 +530,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());
   }
 }
 
@@ -936,3 +928,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/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_interfaces/node_base.cpp

@@ -44,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.
@@ -80,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;
@@ -160,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);
-    }
   }
 }
 
@@ -207,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
@@ -265,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_services.cpp

@@ -41,14 +41,12 @@ NodeServices::add_service(
   }
 
   // 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();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on service creation: ") + ex.what());
   }
 }
 
@@ -68,14 +66,12 @@ NodeServices::add_client(
   }
 
   // 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();
+  } 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

@@ -41,11 +41,15 @@ NodeTimers::add_timer(
   } else {
     node_base_->get_default_callback_group()->add_timer(timer);
   }
-  if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+
+  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 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

@@ -14,9 +14,18 @@
 
 #include "rclcpp/node_interfaces/node_topics.hpp"
 
+#include <stdexcept>
 #include <string>
 
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/qos.hpp"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -55,18 +64,18 @@ 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();
+  } catch (const rclcpp::exceptions::RCLError & ex) {
+    throw std::runtime_error(
+            std::string("failed to notify wait set on publisher creation: ") + ex.what());
   }
 }
 
@@ -97,7 +106,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);
   }
 
@@ -108,13 +118,12 @@ 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();
+  } 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

@@ -41,14 +41,12 @@ NodeWaitables::add_waitable(
   }
 
   // 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();
+  } 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/publisher_base.cpp

@@ -22,6 +22,7 @@
 #include <mutex>
 #include <sstream>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rcutils/logging_macros.h"
@@ -99,6 +100,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 +160,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,6 +17,7 @@
 #include <cstdio>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/exceptions.hpp"
@@ -39,6 +40,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 +85,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 +124,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 +292,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 +301,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 +338,19 @@ 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");
+  }
+}

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);
     }
   }
 
@@ -420,14 +394,13 @@ private:
       node_parameters_,
       node_topics_,
       "/clock",
-      rclcpp::QoS(KeepLast(1)).best_effort(),
-      [state = std::weak_ptr<NodeState>(this->shared_from_this())](
-        std::shared_ptr<const rosgraph_msgs::msg::Clock> msg) {
-        if (auto state_ptr = state.lock()) {
-          state_ptr->clock_cb(msg);
+      qos_,
+      [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()

rclcpp/src/rclcpp/timer.cpp

@@ -119,7 +119,9 @@ TimerBase::time_until_trigger()
   int64_t time_until_next_call = 0;
   rcl_ret_t ret = rcl_timer_get_time_until_next_call(
     timer_handle_.get(), &time_until_next_call);
-  if (ret != RCL_RET_OK) {
+  if (ret == RCL_RET_TIMER_CANCELED) {
+    return std::chrono::nanoseconds::max();
+  } else if (ret != RCL_RET_OK) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
   }
   return std::chrono::nanoseconds(time_until_next_call);

rclcpp/src/rclcpp/utilities.cpp

@@ -33,7 +33,7 @@ namespace rclcpp
 void
 init(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const InitOptions & init_options,
   SignalHandlerOptions signal_handler_options)
 {
@@ -71,7 +71,7 @@ uninstall_signal_handlers()
 static
 std::vector<std::string>
 _remove_ros_arguments(
-  char const * const argv[],
+  char const * const * argv,
   const rcl_arguments_t * args,
   rcl_allocator_t alloc)
 {
@@ -112,7 +112,7 @@ _remove_ros_arguments(
 std::vector<std::string>
 init_and_remove_ros_arguments(
   int argc,
-  char const * const argv[],
+  char const * const * argv,
   const InitOptions & init_options)
 {
   init(argc, argv, init_options);
@@ -123,7 +123,7 @@ init_and_remove_ros_arguments(
 }
 
 std::vector<std::string>
-remove_ros_arguments(int argc, char const * const argv[])
+remove_ros_arguments(int argc, char const * const * argv)
 {
   rcl_allocator_t alloc = rcl_get_default_allocator();
   rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();

rclcpp/src/rclcpp/waitable.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <stdexcept>
+
 #include "rclcpp/waitable.hpp"
 
 using rclcpp::Waitable;
@@ -57,3 +59,21 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+Waitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
+{
+  (void)callback;
+
+  throw std::runtime_error(
+          "Custom waitables should override set_on_ready_callback "
+          "if they want to use it.");
+}
+
+void
+Waitable::clear_on_ready_callback()
+{
+  throw std::runtime_error(
+          "Custom waitables should override clear_on_ready_callback if they "
+          "want to use it and make sure to call it on the waitable destructor.");
+}

rclcpp/test/benchmark/benchmark_client.cpp

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

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -78,6 +78,7 @@ BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
       publishers[i]->publish(empty_msgs);
@@ -104,6 +105,7 @@ BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark:
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
       publishers[i]->publish(empty_msgs);
@@ -142,6 +144,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benc
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -154,6 +157,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -165,6 +169,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(bench
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -176,6 +181,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be
 {
   rclcpp::executors::MultiThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -189,6 +195,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::StaticSingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     executor.add_node(node);
     st.PauseTiming();
     executor.remove_node(node);
@@ -202,6 +209,7 @@ BENCHMARK_F(
 {
   rclcpp::executors::StaticSingleThreadedExecutor executor;
   for (auto _ : st) {
+    (void)_;
     st.PauseTiming();
     executor.add_node(node);
     st.ResumeTiming();
@@ -228,6 +236,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     // static_single_thread_executor has a special design. We need to add/remove the node each
     // time you call spin
     st.PauseTiming();
@@ -265,6 +274,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -294,6 +304,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -317,6 +328,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     auto ret = rclcpp::executors::spin_node_until_future_complete(
       executor, node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
@@ -342,6 +354,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
     if (ret != rclcpp::FutureReturnCode::SUCCESS) {
       st.SkipWithError(rcutils_get_error_string().str);
@@ -383,6 +396,7 @@ BENCHMARK_F(
   reset_heap_counters();
 
   for (auto _ : st) {
+    (void)_;
     std::shared_ptr<void> data = entities_collector_->take_data();
     entities_collector_->execute(data);
   }

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

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