Avoid unecessary creation of MultiThreadedExecutor (#3090 ) (#3096 )

(cherry picked from commit 8cd4d47ec5) Signed-off-by: solonovamax <solonovamax@12oclockpoint.com> Co-authored-by: solo <solonovamax@12oclockpoint.com>
Fix component registering in subdirectories (#3064 ) (#3076 )
2026-03-13 12:58:13 +01:00 · 2026-02-23 10:19:31 +01:00 · 2026-02-19 10:01:41 +01:00 · 2026-02-13 09:48:27 +01:00 · 2026-02-09 15:02:40 +01:00 · 2026-02-09 15:02:35 +01:00
214 changed files with 9926 additions and 1778 deletions
--- a/rclcpp/CHANGELOG.rst
+++ b/rclcpp/CHANGELOG.rst
@@ -2,6 +2,188 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^

+16.0.18 (2026-02-09)
+--------------------
+* print warning message on owner node if the parameter operation fails. (backport `#3037 <https://github.com/ros2/rclcpp/issues/3037>`_) (`#3040 <https://github.com/ros2/rclcpp/issues/3040>`_)
+* fix context in wait for message wait set (`#3030 <https://github.com/ros2/rclcpp/issues/3030>`_) (`#3033 <https://github.com/ros2/rclcpp/issues/3033>`_)
+* Improve the robustness of the TopicEndpointInfo constructor (backport `#3013 <https://github.com/ros2/rclcpp/issues/3013>`_) (`#3016 <https://github.com/ros2/rclcpp/issues/3016>`_)
+* Contributors: mergify[bot]
+
+16.0.17 (2025-12-23)
+--------------------
+* Unified Node Interfaces: Add const version of get_node_x_interface() (`#3006 <https://github.com/ros2/rclcpp/issues/3006>`_) (`#3010 <https://github.com/ros2/rclcpp/issues/3010>`_)
+* [Humble] Implement Unified Node Interface (NodeInterfaces class) (backport `#2041 <https://github.com/ros2/rclcpp/issues/2041>`_) (`#3002 <https://github.com/ros2/rclcpp/issues/3002>`_)
+* remove I/O from signal handler. (`#3000 <https://github.com/ros2/rclcpp/issues/3000>`_) (`#3005 <https://github.com/ros2/rclcpp/issues/3005>`_)
+* correct test function descriptions (backport `#2970 <https://github.com/ros2/rclcpp/issues/2970>`_) (`#2994 <https://github.com/ros2/rclcpp/issues/2994>`_)
+* Contributors: fabianhirmann, mergify[bot]
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+* Allow for implicitly convertable loggers as well (`#2922 <https://github.com/ros2/rclcpp/issues/2922>`_) (`#2936 <https://github.com/ros2/rclcpp/issues/2936>`_) (`#2938 <https://github.com/ros2/rclcpp/issues/2938>`_)
+* Fix: improve exception context for parameter_value_from (backport `#2917 <https://github.com/ros2/rclcpp/issues/2917>`_) (`#2921 <https://github.com/ros2/rclcpp/issues/2921>`_)
+* Removed warning test_qos (`#2859 <https://github.com/ros2/rclcpp/issues/2859>`_) (`#2925 <https://github.com/ros2/rclcpp/issues/2925>`_)
+* Add qos parameter for wait_for_message function (`#2903 <https://github.com/ros2/rclcpp/issues/2903>`_) (`#2907 <https://github.com/ros2/rclcpp/issues/2907>`_)
+* Contributors: mergify[bot]
+
+16.0.14 (2025-07-16)
+--------------------
+* fix test_publisher_with_system_default_qos. (backport `#2881 <https://github.com/ros2/rclcpp/issues/2881>`_) (`#2882 <https://github.com/ros2/rclcpp/issues/2882>`_)
+* Contributors: mergify[bot]
+
+16.0.13 (2025-06-23)
+--------------------
+* Fix for memory leaks in rclcpp::SerializedMessage (`#2861 <https://github.com/ros2/rclcpp/issues/2861>`_) (`#2865 <https://github.com/ros2/rclcpp/issues/2865>`_)
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* QoSInitialization::from_rmw does not validate invalid history policy … (backport `#2841 <https://github.com/ros2/rclcpp/issues/2841>`_) (`#2844 <https://github.com/ros2/rclcpp/issues/2844>`_)
+* throws std::invalid_argument if ParameterEvent is NULL. (`#2814 <https://github.com/ros2/rclcpp/issues/2814>`_) (`#2824 <https://github.com/ros2/rclcpp/issues/2824>`_)
+* remove redundant typesupport check in serialization module (`#2808 <https://github.com/ros2/rclcpp/issues/2808>`_) (`#2816 <https://github.com/ros2/rclcpp/issues/2816>`_)
+* Contributors: mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+* doc: Added warning to not instantiate Clock directly with RCL_ROS_TIME (`#2768 <https://github.com/ros2/rclcpp/issues/2768>`_) (`#2770 <https://github.com/ros2/rclcpp/issues/2770>`_)
+* apply actual QoS from rmw to the IPC publisher. (backport `#2707 <https://github.com/ros2/rclcpp/issues/2707>`_) (`#2711 <https://github.com/ros2/rclcpp/issues/2711>`_)
+* Adding in topic name to logging on IPC issues (`#2706 <https://github.com/ros2/rclcpp/issues/2706>`_) (`#2709 <https://github.com/ros2/rclcpp/issues/2709>`_)
+* Contributors: mergify[bot]
+
+16.0.11 (2024-11-25)
+--------------------
+* Fix subscription.is_serialized() for callbacks with message info (`#1950 <https://github.com/ros2/rclcpp/issues/1950>`_) (`#2622 <https://github.com/ros2/rclcpp/issues/2622>`_)
+* Use the same context for the specified node in rclcpp::spin functions… (`#2618 <https://github.com/ros2/rclcpp/issues/2618>`_) (`#2620 <https://github.com/ros2/rclcpp/issues/2620>`_)
+* Contributors: mergify[bot], roscan-tech
+
+16.0.10 (2024-07-26)
+--------------------
+* Add test creating two content filter topics with the same topic name (`#2546 <https://github.com/ros2/rclcpp/issues/2546>`_) (`#2549 <https://github.com/ros2/rclcpp/issues/2549>`_) (`#2551 <https://github.com/ros2/rclcpp/issues/2551>`_)
+* Contributors: mergify[bot]
+
+16.0.9 (2024-05-15)
+-------------------
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_) (`#2529 <https://github.com/ros2/rclcpp/issues/2529>`_)
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_) (`#2485 <https://github.com/ros2/rclcpp/issues/2485>`_)
+* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_) (`#2459 <https://github.com/ros2/rclcpp/issues/2459>`_)
+* Contributors: Tamaki Nishino, mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_) (`#2394 <https://github.com/ros2/rclcpp/issues/2394>`_)
+* Contributors: gentoo90
+
+16.0.7 (2023-11-13)
+-------------------
+* Disable the loaned messages inside the executor. (backport `#2335 <https://github.com/ros2/rclcpp/issues/2335>`_) (`#2364 <https://github.com/ros2/rclcpp/issues/2364>`_)
+* Add missing 'enable_rosout' comments (`#2345 <https://github.com/ros2/rclcpp/issues/2345>`_) (`#2347 <https://github.com/ros2/rclcpp/issues/2347>`_)
+* address rate related flaky tests. (`#2329 <https://github.com/ros2/rclcpp/issues/2329>`_) (`#2342 <https://github.com/ros2/rclcpp/issues/2342>`_)
+* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 <https://github.com/ros2/rclcpp/issues/2316>`_) (`#2321 <https://github.com/ros2/rclcpp/issues/2321>`_)
+* Fix C++20 allocator construct deprecation (`#2292 <https://github.com/ros2/rclcpp/issues/2292>`_) (`#2319 <https://github.com/ros2/rclcpp/issues/2319>`_)
+* Contributors: mergify[bot]
+
+16.0.6 (2023-09-19)
+-------------------
+* Topic correct typeadapter deduction (`#2294 <https://github.com/ros2/rclcpp/issues/2294>`_) (`#2297 <https://github.com/ros2/rclcpp/issues/2297>`_)
+* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_) (`#2275 <https://github.com/ros2/rclcpp/issues/2275>`_)
+* Do not crash Executor when send_response fails due to client failure. (`#2276 <https://github.com/ros2/rclcpp/issues/2276>`_) (`#2280 <https://github.com/ros2/rclcpp/issues/2280>`_)
+* Contributors: mergify[bot]
+
+16.0.5 (2023-07-17)
+-------------------
+* warning: comparison of integer expressions of different signedness (`#2219 <https://github.com/ros2/rclcpp/issues/2219>`_) (`#2223 <https://github.com/ros2/rclcpp/issues/2223>`_)
+* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_) (`#2167 <https://github.com/ros2/rclcpp/issues/2167>`_)
+* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_) (`#2210 <https://github.com/ros2/rclcpp/issues/2210>`_)
+* Contributors: Tomoya Fujita, mergify[bot]
+
+16.0.4 (2023-04-25)
+-------------------
+* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_) (`#2131 <https://github.com/ros2/rclcpp/issues/2131>`_)
+* Contributors: mergify[bot]
+
+16.0.3 (2023-01-10)
+-------------------
+* Fix SharedFuture from async_send_request never becomes valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_) (`#2076 <https://github.com/ros2/rclcpp/issues/2076>`_)
+* do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_) (`#2070 <https://github.com/ros2/rclcpp/issues/2070>`_)
+* fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_) (`#2065 <https://github.com/ros2/rclcpp/issues/2065>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_) (`#2033 <https://github.com/ros2/rclcpp/issues/2033>`_)
+* Contributors: mergify[bot]
+
+16.0.2 (2022-11-07)
+-------------------
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_) (`#2026 <https://github.com/ros2/rclcpp/issues/2026>`_)
+* use regex for wildcard matching (backport `#1839 <https://github.com/ros2/rclcpp/issues/1839>`_) (`#1986 <https://github.com/ros2/rclcpp/issues/1986>`_)
+* Drop wrong template specialization (`#1926 <https://github.com/ros2/rclcpp/issues/1926>`_) (`#1937 <https://github.com/ros2/rclcpp/issues/1937>`_)
+* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_) (`#1932 <https://github.com/ros2/rclcpp/issues/1932>`_)
+* Contributors: mergify[bot]
+
+16.0.1 (2022-04-13)
+-------------------
+* remove DEFINE_CONTENT_FILTER cmake option (`#1914 <https://github.com/ros2/rclcpp/issues/1914>`_)
+* Contributors: Chen Lihui
+
+16.0.0 (2022-04-08)
+-------------------
+* remove things that were deprecated during galactic (`#1913 <https://github.com/ros2/rclcpp/issues/1913>`_)
+* Contributors: William Woodall
+
+15.4.0 (2022-04-05)
+-------------------
+* add take_data_by_entity_id API to waitable (`#1892 <https://github.com/ros2/rclcpp/issues/1892>`_)
+* add content-filtered-topic interfaces (`#1561 <https://github.com/ros2/rclcpp/issues/1561>`_)
+* Contributors: Alberto Soragna, Chen Lihui
+
+15.3.0 (2022-03-30)
+-------------------
+* [NodeParameters] Set name in param info pre-check (`#1908 <https://github.com/ros2/rclcpp/issues/1908>`_)
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_)
+* Add publish by loaned message in GenericPublisher (`#1856 <https://github.com/ros2/rclcpp/issues/1856>`_)
+* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard
+
+15.2.0 (2022-03-24)
+-------------------
+* Add missing ament dependency on rcl_interfaces (`#1903 <https://github.com/ros2/rclcpp/issues/1903>`_)
+* Update data callback tests to account for all published samples (`#1900 <https://github.com/ros2/rclcpp/issues/1900>`_)
+* Increase timeout for acknowledgments to account for slower Connext settings (`#1901 <https://github.com/ros2/rclcpp/issues/1901>`_)
+* clang-tidy: explicit constructors (`#1782 <https://github.com/ros2/rclcpp/issues/1782>`_)
+* Add client/service QoS getters (`#1784 <https://github.com/ros2/rclcpp/issues/1784>`_)
+* Fix a bunch more rosdoc2 issues in rclcpp. (`#1897 <https://github.com/ros2/rclcpp/issues/1897>`_)
+* time_until_trigger returns max time if timer is cancelled (`#1893 <https://github.com/ros2/rclcpp/issues/1893>`_)
+* Micro-optimizations in rclcpp (`#1896 <https://github.com/ros2/rclcpp/issues/1896>`_)
+* Contributors: Andrea Sorbini, Chris Lalancette, Mauro Passerino, Scott K Logan, William Woodall
+
+15.1.0 (2022-03-01)
+-------------------
+* spin_all with a zero timeout. (`#1878 <https://github.com/ros2/rclcpp/issues/1878>`_)
+* Add RMW listener APIs (`#1579 <https://github.com/ros2/rclcpp/issues/1579>`_)
+* Remove fastrtps customization on tests (`#1887 <https://github.com/ros2/rclcpp/issues/1887>`_)
+* Install headers to include/${PROJECT_NAME} (`#1888 <https://github.com/ros2/rclcpp/issues/1888>`_)
+* Use ament_generate_version_header (`#1886 <https://github.com/ros2/rclcpp/issues/1886>`_)
+* use universal reference to support rvalue. (`#1883 <https://github.com/ros2/rclcpp/issues/1883>`_)
+* fix one subscription can wait_for_message twice (`#1870 <https://github.com/ros2/rclcpp/issues/1870>`_)
+* Add return value version of get_parameter_or (`#1813 <https://github.com/ros2/rclcpp/issues/1813>`_)
+* Cleanup time source object lifetimes (`#1867 <https://github.com/ros2/rclcpp/issues/1867>`_)
+* add is_spinning() method to executor base class
+* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Kenji Miyake, Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS
+
+15.0.0 (2022-01-14)
+-------------------
+* Cleanup the TypeAdapt tests (`#1858 <https://github.com/ros2/rclcpp/issues/1858>`_)
+* Cleanup includes (`#1857 <https://github.com/ros2/rclcpp/issues/1857>`_)
+* Fix include order and relative paths for cpplint (`#1859 <https://github.com/ros2/rclcpp/issues/1859>`_)
+* Rename stringstream in macros to a more unique name (`#1862 <https://github.com/ros2/rclcpp/issues/1862>`_)
+* Add non transform capabilities for intra-process (`#1849 <https://github.com/ros2/rclcpp/issues/1849>`_)
+* Fix rclcpp documentation build (`#1779 <https://github.com/ros2/rclcpp/issues/1779>`_)
+* Contributors: Chris Lalancette, Doug Smith, Gonzo, Jacob Perron, Michel Hidalgo
+
+14.1.0 (2022-01-05)
+-------------------
+* Use UninitializedStaticallyTypedParameterException (`#1689 <https://github.com/ros2/rclcpp/issues/1689>`_)
+* Add wait_for_all_acked support (`#1662 <https://github.com/ros2/rclcpp/issues/1662>`_)
+* Add tests for function templates of declare_parameter (`#1747 <https://github.com/ros2/rclcpp/issues/1747>`_)
+* Contributors: Barry Xu, Bi0T1N, M. Mostafa Farzan
+
 14.0.0 (2021-12-17)
 -------------------
 * Fixes for uncrustify 0.72 (`#1844 <https://github.com/ros2/rclcpp/issues/1844>`_)
--- a/rclcpp/CMakeLists.txt
+++ b/rclcpp/CMakeLists.txt
@@ -187,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"
@@ -217,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)
@@ -247,7 +251,7 @@ ament_package()

 install(
  DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
-  DESTINATION include
+  DESTINATION include/${PROJECT_NAME}
 )

 if(TEST cppcheck)
@@ -255,4 +259,4 @@ if(TEST cppcheck)
  set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()

-ament_cmake_gen_version_h()
+ament_generate_version_header(${PROJECT_NAME})
--- a/rclcpp/Doxyfile
+++ b/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"
--- a/rclcpp/QUALITY_DECLARATION.md
+++ b/rclcpp/QUALITY_DECLARATION.md
@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).

 # rclcpp Quality Declaration

 The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.

-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.

 ## Version Policy [1]

@@ -55,7 +55,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d

 ### Continuous Integration [2.iv]

-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)

 Currently nightly results can be seen here:

@@ -213,7 +213,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab

 ## Platform Support [6]

-`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.

 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/)
@@ -225,4 +225,4 @@ Currently nightly build status can be seen here:

 ### Vulnerability Disclosure Policy [7.i]

-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_

+#include <cstring>
 #include <memory>

 #include "rcl/allocator.h"
@@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }

+template<typename Alloc>
+void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  size_t size = number_of_elem * size_of_elem;
+  void * allocated_memory =
+    std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+  if (allocated_memory) {
+    std::memset(allocated_memory, 0, size);
+  }
+  return allocated_memory;
+}
+
 template<typename T, typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
@@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
  rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
  rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.zero_allocate = &retyped_zero_allocate<Alloc>;
  rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
  rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
  rcl_allocator.state = &allocator;
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/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();
  }
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -354,12 +354,6 @@ public:
    allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
  }

-  [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]]
-  explicit
-  AnySubscriptionCallback(std::shared_ptr<AllocatorT> allocator)  // NOLINT[runtime/explicit]
-  : AnySubscriptionCallback(*NotNull<AllocatorT>(allocator.get(), "invalid allocator").pointer)
-  {}
-
  AnySubscriptionCallback(const AnySubscriptionCallback &) = default;

  /// Generic function for setting the callback.
@@ -476,6 +470,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 +668,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 +687,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 +798,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 +812,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]
@@ -888,7 +950,13 @@ public:
      std::holds_alternative<UniquePtrSerializedMessageCallback>(callback_variant_) ||
      std::holds_alternative<SharedConstPtrSerializedMessageCallback>(callback_variant_) ||
      std::holds_alternative<ConstRefSharedConstPtrSerializedMessageCallback>(callback_variant_) ||
-      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_);
+      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<UniquePtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<SharedConstPtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSharedConstPtrSerializedMessageWithInfoCallback>(
+      callback_variant_) ||
+      std::holds_alternative<SharedPtrSerializedMessageWithInfoCallback>(callback_variant_);
  }

  void
--- a/rclcpp/include/rclcpp/callback_group.hpp
+++ b/rclcpp/include/rclcpp/callback_group.hpp
@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_

 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>

 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -95,6 +98,10 @@ public:
    CallbackGroupType group_type,
    bool automatically_add_to_executor_with_node = true);

+  /// Default destructor.
+  RCLCPP_PUBLIC
+  ~CallbackGroup();
+
  template<typename Function>
  rclcpp::SubscriptionBase::SharedPtr
  find_subscription_ptrs_if(Function func) const
@@ -138,6 +145,14 @@ public:
  const CallbackGroupType &
  type() const;

+  RCLCPP_PUBLIC
+  void collect_all_ptrs(
+    std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+    std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+    std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+    std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+    std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const;
+
  /// Return a reference to the 'associated with executor' atomic boolean.
  /**
   * When a callback group is added to an executor this boolean is checked
@@ -163,6 +178,16 @@ public:
  bool
  automatically_add_to_executor_with_node() const;

+  /// Defer creating the notify guard condition and return it.
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
  RCLCPP_DISABLE_COPY(CallbackGroup)

@@ -205,6 +230,9 @@ protected:
  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
  std::atomic_bool can_be_taken_from_;
  const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;

 private:
  template<typename TypeT, typename Function>
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -19,6 +19,7 @@
 #include <future>
 #include <unordered_map>
 #include <memory>
+#include <mutex>
 #include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
@@ -29,20 +30,23 @@

 #include "rcl/client.h"
 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
 #include "rcl/wait.h"

+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"

-#include "rcutils/logging_macros.h"
-
 #include "rmw/error_handling.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"

 namespace rclcpp
@@ -215,6 +219,122 @@ public:
  bool
  exchange_in_use_by_wait_set_state(bool in_use_state);

+  /// Get the actual request publsher QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual request publsher qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_request_publisher_actual_qos() const;
+
+  /// Get the actual response subscription QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the client, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual response subscription qos settings.
+   * \throws std::runtime_error if failed to get qos settings
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_response_subscription_actual_qos() const;
+
+  /// Set a callback to be called when each new response is received.
+  /**
+   * The callback receives a size_t which is the number of responses received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if responses were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the client
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_client_set_on_new_response_callback
+   * \sa rcl_client_set_on_new_response_callback
+   *
+   * \param[in] callback functor to be called when a new response is received
+   */
+  void
+  set_on_new_response_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_response_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_responses) {
+        try {
+          callback(number_of_responses);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new response' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::ClientBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new response' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_response_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_response_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_response_callback_));
+  }
+
+  /// Unset the callback registered for new responses, if any.
+  void
+  clear_on_new_response_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_response_callback_) {
+      set_on_new_response_callback(nullptr, nullptr);
+      on_new_response_callback_ = nullptr;
+    }
+  }
+
 protected:
  RCLCPP_DISABLE_COPY(ClientBase)

@@ -230,13 +350,21 @@ protected:
  const rcl_node_t *
  get_rcl_node_handle() const;

+  RCLCPP_PUBLIC
+  void
+  set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data);
+
  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
  std::shared_ptr<rcl_node_t> node_handle_;
  std::shared_ptr<rclcpp::Context> context_;
+  rclcpp::Logger node_logger_;

  std::shared_ptr<rcl_client_t> client_handle_;

  std::atomic<bool> in_use_by_wait_set_{false};
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
 };

 template<typename ServiceT>
@@ -641,7 +769,9 @@ public:
    auto old_size = pending_requests_.size();
    for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
      if (it->second.first < time_point) {
-        pruned_requests->push_back(it->first);
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
        it = pending_requests_.erase(it);
      } else {
        ++it;
@@ -664,16 +794,14 @@ protected:
  async_send_request_impl(const Request & request, CallbackInfoVariant value)
  {
    int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
    if (RCL_RET_OK != ret) {
      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
    }
-    {
-      std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-      pending_requests_.try_emplace(
-        sequence_number,
-        std::make_pair(std::chrono::system_clock::now(), std::move(value)));
-    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
    return sequence_number;
  }

@@ -688,7 +816,7 @@ protected:
        "Received invalid sequence number. Ignoring...");
      return std::nullopt;
    }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
    this->pending_requests_.erase(request_number);
    return value;
  }
--- a/rclcpp/include/rclcpp/clock.hpp
+++ b/rclcpp/include/rclcpp/clock.hpp
@@ -60,6 +60,13 @@ public:
  /**
   * Initializes the clock instance with the given clock_type.
   *
+   * WARNING Don't instantiate a clock using RCL_ROS_TIME directly,
+   * unless you really know what you are doing. By default no TimeSource
+   * is attached to a new clock. This will lead to the unexpected behavior,
+   * that your RCL_ROS_TIME will run always on system time. If you want
+   * a RCL_ROS_TIME use Node::get_clock(), or make sure to attach a
+   * TimeSource yourself.
+   *
   * \param clock_type type of the clock.
   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
   */
@@ -137,6 +144,51 @@ public:
    Duration rel_time,
    Context::SharedPtr context = contexts::get_global_default_context());

+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
+
  /**
   * Returns the clock of the type `RCL_ROS_TIME` is active.
   *
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>

 #include "rcl/context.h"
 #include "rcl/guard_condition.h"
@@ -127,7 +128,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.
--- a/rclcpp/include/rclcpp/create_publisher.hpp
+++ b/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 = (
--- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp
+++ b/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_
--- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp
+++ b/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) \
--- a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp
+++ b/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.
--- a/rclcpp/include/rclcpp/detail/template_contains.hpp
+++ b/rclcpp/include/rclcpp/detail/template_contains.hpp
@@ -0,0 +1,47 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+
+#include <type_traits>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given T is contained in the list Us.
+template<typename T, typename ... Us>
+struct template_contains;
+
+template<typename ... Args>
+inline constexpr bool template_contains_v = template_contains<Args ...>::value;
+
+template<typename T, typename NextT, typename ... Us>
+struct template_contains<T, NextT, Us ...>
+{
+  enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
+};
+
+template<typename T>
+struct template_contains<T>
+{
+  enum { value = false };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
--- a/rclcpp/include/rclcpp/detail/template_unique.hpp
+++ b/rclcpp/include/rclcpp/detail/template_unique.hpp
@@ -0,0 +1,49 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/detail/template_contains.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given list Ts contains unique types.
+template<typename ... Ts>
+struct template_unique;
+
+template<typename ... Args>
+inline constexpr bool template_unique_v = template_unique<Args ...>::value;
+
+template<typename NextT, typename ... Ts>
+struct template_unique<NextT, Ts ...>
+{
+  enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
+};
+
+template<typename T>
+struct template_unique<T>
+{
+  enum { value = true };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
--- a/rclcpp/include/rclcpp/detail/utilities.hpp
+++ b/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);

--- a/rclcpp/include/rclcpp/duration.hpp
+++ b/rclcpp/include/rclcpp/duration.hpp
@@ -38,13 +38,6 @@ public:
   */
  Duration(int32_t seconds, uint32_t nanoseconds);

-  /// Construct duration from the specified nanoseconds.
-  [[deprecated(
-    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
-    "rclcpp::Duration::from_nanoseconds(int64_variable);"
-    "rclcpp::Duration(0ns);")]]
-  explicit Duration(rcl_duration_value_t nanoseconds);
-
  /// Construct duration from the specified std::chrono::nanoseconds.
  explicit Duration(std::chrono::nanoseconds nanoseconds);

--- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp
+++ b/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")
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/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
@@ -548,14 +560,14 @@ protected:
  virtual void
  spin_once_impl(std::chrono::nanoseconds timeout);

-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
      const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;

-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);

  /// maps callback groups associated to nodes
  WeakCallbackGroupsToNodesMap
--- a/rclcpp/include/rclcpp/executors.hpp
+++ b/rclcpp/include/rclcpp/executors.hpp
@@ -107,7 +107,9 @@ spin_until_future_complete(
  const FutureT & future,
  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
-  rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor executor(options);
  return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }

--- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
+++ b/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,
--- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
+++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp
@@ -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
--- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp
+++ b/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:
--- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp
+++ b/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>

--- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp
+++ b/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>
@@ -90,8 +86,7 @@ public:
    std::lock_guard<std::mutex> lock(mutex_);

    if (!has_data_()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+      return BufferT();
    }

    auto request = std::move(ring_buffer_[read_index_]);
--- a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp
+++ b/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"
--- a/rclcpp/include/rclcpp/experimental/executable_list.hpp
+++ b/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"
--- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
+++ b/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,88 @@ 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));
+          // Last message delivered, break from for loop
+          break;
        } 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, ROSMessageType>::is_specialized::value) {
+        ROSMessageTypeAllocator ros_message_alloc(allocator);
+        auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
+        ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
+        ROSMessageTypeDeleter deleter;
+        allocator::set_allocator_for_deleter(&deleter, &allocator);
+        rclcpp::TypeAdapter<MessageT, ROSMessageType>::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));
+            // Last message delivered, break from for loop
+            break;
+          } 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)));
+          }
+        }
      }
    }
  }
--- a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp
+++ b/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_
--- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp
+++ b/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,16 +102,29 @@ public:
  virtual ~SubscriptionIntraProcess() = default;

  std::shared_ptr<void>
-  take_data()
+  take_data() override
  {
    ConstMessageSharedPtr shared_msg;
    MessageUniquePtr unique_msg;

    if (any_callback_.use_take_shared_method()) {
      shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
    } else {
      unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
    }
+
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+
    return std::static_pointer_cast<void>(
      std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
        std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
@@ -115,9 +132,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:
@@ -134,7 +151,7 @@ protected:
  execute_impl(std::shared_ptr<void> & data)
  {
    if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
    }

    rmw_message_info_t msg_info;
@@ -154,7 +171,7 @@ protected:
    shared_ptr.reset();
  }

-  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
 };

 }  // namespace experimental
--- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp
+++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp
@@ -15,19 +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
@@ -40,6 +38,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 public:
  RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)

+  enum class EntityType : std::size_t
+  {
+    Subscription,
+  };
+
  RCLCPP_PUBLIC
  SubscriptionIntraProcessBase(
    rclcpp::Context::SharedPtr context,
@@ -48,27 +51,35 @@ public:
  : 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
  void
-  add_to_wait_set(rcl_wait_set_t * wait_set);
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;

-  virtual bool
-  is_ready(rcl_wait_set_t * wait_set) = 0;
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override = 0;
+
+  std::shared_ptr<void>
+  take_data() override = 0;
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
+  void
+  execute(std::shared_ptr<void> & data) override = 0;

  virtual
-  std::shared_ptr<void>
-  take_data() = 0;
-
-  virtual void
-  execute(std::shared_ptr<void> & data) = 0;
-
-  virtual bool
+  bool
  use_take_shared_method() const = 0;

  RCLCPP_PUBLIC
@@ -79,14 +90,108 @@ public:
  QoS
  get_actual_qos() const;

+  /// 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 reentrant_mutex_;
+  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_;
 };
--- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp
+++ b/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,50 +78,101 @@ public:
    const std::string & topic_name,
    const rclcpp::QoS & qos_profile,
    rclcpp::IntraProcessBufferType buffer_type)
-  : SubscriptionIntraProcessBase(context, 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);
+      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
  {
-    gc_.trigger();
+    this->gc_.trigger();
  }

  BufferUniquePtr buffer_;
+  SubscribedTypeAllocator subscribed_type_allocator_;
+  SubscribedTypeDeleter subscribed_type_deleter_;
 };

 }  // namespace experimental
--- a/rclcpp/include/rclcpp/function_traits.hpp
+++ b/rclcpp/include/rclcpp/function_traits.hpp
@@ -80,7 +80,9 @@ 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 ...)>>
@@ -97,7 +99,9 @@ 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>>
@@ -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 ...)>>
--- a/rclcpp/include/rclcpp/generic_publisher.hpp
+++ b/rclcpp/include/rclcpp/generic_publisher.hpp
@@ -31,6 +31,8 @@
 #include "rclcpp/typesupport_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"

+#include "rmw/rmw.h"
+
 namespace rclcpp
 {

@@ -60,7 +62,6 @@ public:
   * \param topic_name Topic name
   * \param topic_type Topic type
   * \param qos %QoS settings
-   * \param callback Callback for new messages of serialized form
   * \param options %Publisher options.
   * Not all publisher options are currently respected, the only relevant options for this
   * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
@@ -117,9 +118,24 @@ public:
  RCLCPP_PUBLIC
  void publish(const rclcpp::SerializedMessage & message);

+  /**
+   * Publish a rclcpp::SerializedMessage via loaned message after de-serialization.
+   *
+   * \param message a serialized message
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can show
+   */
+  RCLCPP_PUBLIC
+  void publish_as_loaned_msg(const rclcpp::SerializedMessage & message);
+
 private:
  // The type support library should stay loaded, so it is stored in the GenericPublisher
  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+
+  void * borrow_loaned_message();
+  void deserialize_message(
+    const rmw_serialized_message_t & serialized_message,
+    void * deserialized_msg);
+  void publish_loaned_message(void * loaned_message);
 };

 }  // namespace rclcpp
--- a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp
+++ b/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_
--- a/rclcpp/include/rclcpp/guard_condition.hpp
+++ b/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.
@@ -96,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
--- a/rclcpp/include/rclcpp/logger.hpp
+++ b/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.
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -100,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);
 };

--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -18,6 +18,7 @@
 #include <memory>
 #include <stdexcept>

+#include "rcl/allocator.h"
 #include "rcl/types.h"

 #include "rclcpp/allocator/allocator_common.hpp"
@@ -61,7 +62,12 @@ public:
    message_allocator_ = std::make_shared<MessageAlloc>();
    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
    buffer_allocator_ = std::make_shared<BufferAlloc>();
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
  }

  explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -69,7 +75,12 @@ public:
    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
    buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
  }

  virtual ~MessageMemoryStrategy() = default;
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -395,22 +395,6 @@ public:
    rcl_interfaces::msg::ParameterDescriptor{},
    bool ignore_override = false);

-  /// Declare a parameter
-  [[deprecated(
-    "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
-    "If you want to declare a parameter that won't change type without a default value use:\n" \
-    "`node->declare_parameter<ParameterT>(name)`, where e.g. ParameterT=int64_t.\n\n" \
-    "If you want to declare a parameter that can dynamically change type use:\n" \
-    "```\n" \
-    "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
-    "descriptor.dynamic_typing = true;\n" \
-    "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
-    "```"
-  )]]
-  RCLCPP_PUBLIC
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name);
-
  /// Declare and initialize a parameter with a type.
  /**
   * See the non-templated declare_parameter() on this class for details.
@@ -720,6 +704,24 @@ public:
    ParameterT & parameter,
    const ParameterT & alternative_value) const;

+  /// Return the parameter value, or the "alternative_value" if not set.
+  /**
+   * If the parameter was not set, then the "alternative_value" argument is returned.
+   *
+   * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception.
+   *
+   * In all cases, the parameter is never set or declared within the node.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns The value of the parameter.
+   */
+  template<typename ParameterT>
+  ParameterT
+  get_parameter_or(
+    const std::string & name,
+    const ParameterT & alternative_value) const;
+
  /// Return the parameters by the given parameter names.
  /**
   * Like get_parameters(), this method may throw the
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/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.
--- a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp
@@ -0,0 +1,210 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+
+#include <memory>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// Support and Helper template classes for the NodeInterfaces class.
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n);
+
+/// Stores the interfaces in a tuple, provides constructors, and getters.
+template<typename ... InterfaceTs>
+struct NodeInterfacesStorage
+{
+  template<typename NodeT>
+  NodeInterfacesStorage(NodeT & node)  // NOLINT(runtime/explicit)
+  : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
+  {}
+
+  explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
+  : interfaces_(args ...)
+  {}
+
+  /// Individual Node Interface non-const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<NodeInterfaceT>
+  get()
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+  /// Individual Node Interface const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<const NodeInterfaceT>
+  get() const
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+protected:
+  std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
+};
+
+/// Prototype of NodeInterfacesSupports.
+/**
+ * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
+ * if NodeInterfacesSupport is specialized for T, the is_supported should be
+ * set to std::true_type, but by default it is std::false_type, which will
+ * lead to a compiler error when trying to use T with NodeInterfaces.
+ */
+template<typename StorageClassT, typename ... Ts>
+struct NodeInterfacesSupports;
+
+/// Prototype of NodeInterfacesSupportCheck template meta-function.
+/**
+ * This meta-function checks that all the types given are supported,
+ * throwing a more human-readable error if an unsupported type is used.
+ */
+template<typename StorageClassT, typename ... InterfaceTs>
+struct NodeInterfacesSupportCheck;
+
+/// Iterating specialization that ensures classes are supported and inherited.
+template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
+  : public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
+{
+  static_assert(
+    NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
+    "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
+};
+
+/// Terminating case when there are no more "RemainingInterfaceTs".
+template<typename StorageClassT>
+struct NodeInterfacesSupportCheck<StorageClassT>
+{};
+
+/// Default specialization, needs to be specialized for each supported interface.
+template<typename StorageClassT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupports
+{
+  // Specializations need to set this to std::true_type in addition to other interfaces.
+  using is_supported = std::false_type;
+};
+
+/// Terminating specialization of NodeInterfacesSupports.
+template<typename StorageClassT>
+struct NodeInterfacesSupports<StorageClassT>
+  : public StorageClassT
+{
+  /// Perfect forwarding constructor to get arguments down to StorageClassT.
+  template<typename ... ArgsT>
+  explicit NodeInterfacesSupports(ArgsT && ... args)
+  : StorageClassT(std::forward<ArgsT>(args) ...)
+  {}
+};
+
+// Helper functions to initialize the tuple in NodeInterfaces.
+
+template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
+void
+init_element(TupleT & t, NodeT & n)
+{
+  std::get<std::shared_ptr<ElementT>>(t) =
+    NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
+}
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n)
+{
+  using StorageClassT = NodeInterfacesStorage<Ts ...>;
+  std::tuple<std::shared_ptr<Ts>...> t;
+  (init_element<StorageClassT, Ts>(t, n), ...);
+  return t;
+}
+
+/// Macro for creating specializations with less boilerplate.
+/**
+ * You can use this macro to add support for your interface class if:
+ *
+ * - The standard getter is get_node_{NodeInterfaceName}_interface(), and
+ * - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
+ *
+ * Examples of using this can be seen in the standard node interface headers
+ * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
+ *
+ *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+ *
+ * If your interface has a non-standard getter, or you want to instrument it or
+ * something like that, then you'll need to create your own specialization of
+ * the NodeInterfacesSupports struct without this macro.
+ */
+#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
+  namespace rclcpp::node_interfaces::detail { \
+  template<typename StorageClassT, typename ... RemainingInterfaceTs> \
+  struct NodeInterfacesSupports< \
+    StorageClassT, \
+    NodeInterfaceType, \
+    RemainingInterfaceTs ...> \
+    : public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
+  { \
+    using is_supported = std::true_type; \
+ \
+    template<typename NodeT> \
+    static \
+    std::shared_ptr<NodeInterfaceType> \
+    get_from_node_like(NodeT & node_like) \
+    { \
+      return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
+    } \
+ \
+    /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
+    template<typename ... ArgsT> \
+    explicit NodeInterfacesSupports(ArgsT && ... args) \
+      : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+        std::forward<ArgsT>(args) ...) \
+    {} \
+ \
+    std::shared_ptr<NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+ \
+    std::shared_ptr<const NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() const \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
@@ -26,6 +26,7 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -177,4 +178,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp
@@ -17,6 +17,7 @@

 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -50,4 +51,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp
@@ -29,6 +29,7 @@

 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"

@@ -52,12 +53,16 @@ public:
  /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t.
  RCLCPP_PUBLIC
  explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
-  : node_name_(info.node_name),
-    node_namespace_(info.node_namespace),
-    topic_type_(info.topic_type),
-    endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+  : endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
  {
+    if (!info.node_name || !info.node_namespace || !info.topic_type) {
+      throw std::invalid_argument("Constructor TopicEndpointInfo with invalid topic endpoint info");
+    }
+    node_name_ = info.node_name;
+    node_namespace_ = info.node_namespace;
+    topic_type_ = info.topic_type;
+
    std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
  }

@@ -357,6 +362,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 +374,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
@@ -378,4 +387,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp
@@ -0,0 +1,171 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+
+#include <memory>
+
+#include "rclcpp/detail/template_unique.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+
+#define ALL_RCLCPP_NODE_INTERFACES \
+  rclcpp::node_interfaces::NodeBaseInterface, \
+  rclcpp::node_interfaces::NodeClockInterface, \
+  rclcpp::node_interfaces::NodeGraphInterface, \
+  rclcpp::node_interfaces::NodeLoggingInterface, \
+  rclcpp::node_interfaces::NodeParametersInterface, \
+  rclcpp::node_interfaces::NodeServicesInterface, \
+  rclcpp::node_interfaces::NodeTimeSourceInterface, \
+  rclcpp::node_interfaces::NodeTimersInterface, \
+  rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeWaitablesInterface
+
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+
+/// A helper class for aggregating node interfaces.
+template<typename ... InterfaceTs>
+class NodeInterfaces
+  : public detail::NodeInterfacesSupportCheck<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >,
+  public detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >
+{
+  static_assert(
+    0 != sizeof ...(InterfaceTs),
+    "must provide at least one interface as a template argument");
+  static_assert(
+    rclcpp::detail::template_unique_v<InterfaceTs ...>,
+    "must provide unique template parameters");
+
+  using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >;
+
+public:
+  /// Create a new NodeInterfaces object using the given node-like object's interfaces.
+  /**
+   * Specify which interfaces you need by passing them as template parameters.
+   *
+   * This allows you to aggregate interfaces from different sources together to pass as a single
+   * aggregate object to any functions that take node interfaces or node-likes, without needing to
+   * templatize that function.
+   *
+   * You may also use this constructor to create a NodeInterfaces that contains a subset of
+   * another NodeInterfaces' interfaces.
+   *
+   * Finally, this class supports implicit conversion from node-like objects, allowing you to
+   * directly pass a node-like to a function that takes a NodeInterfaces object.
+   *
+   * Usage examples:
+   *   ```cpp
+   *   // Suppose we have some function:
+   *   void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
+   *
+   *   // Then we can, explicitly:
+   *   rclcpp::Node node("some_node");
+   *   auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
+   *   fn(ni);
+   *
+   *   // But also:
+   *   fn(node);
+   *
+   *   // Subsetting a NodeInterfaces object also works!
+   *   auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
+   *
+   *   // Or aggregate them (you could aggregate interfaces from disparate node-likes)
+   *   auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
+   *     node->get_node_base_interface(),
+   *     node->get_node_clock_interface()
+   *   )
+   *
+   *   // And then to access the interfaces:
+   *   // Get with get<>
+   *   auto base = ni.get<NodeBaseInterface>();
+   *
+   *   // Or the appropriate getter
+   *   auto clock = ni.get_clock_interface();
+   *   ```
+   *
+   * You may use any of the standard node interfaces that come with rclcpp:
+   *   - rclcpp::node_interfaces::NodeBaseInterface
+   *   - rclcpp::node_interfaces::NodeClockInterface
+   *   - rclcpp::node_interfaces::NodeGraphInterface
+   *   - rclcpp::node_interfaces::NodeLoggingInterface
+   *   - rclcpp::node_interfaces::NodeParametersInterface
+   *   - rclcpp::node_interfaces::NodeServicesInterface
+   *   - rclcpp::node_interfaces::NodeTimeSourceInterface
+   *   - rclcpp::node_interfaces::NodeTimersInterface
+   *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeWaitablesInterface
+   *
+   * Or you use custom interfaces as long as you make a template specialization
+   * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
+   * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
+   *
+   * Usage example:
+   *   ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
+   *
+   * If you choose not to use the helper macro, then you can specialize the
+   * template yourself, but you must:
+   *
+   *   - Provide a template specialization of the get_from_node_like method that gets the interface
+   *     from any node-like that stores the interface, using the node-like's getter
+   *   - Designate the is_supported type as std::true_type using a using directive
+   *   - Provide any number of getter methods to be used to obtain the interface with the
+   *     NodeInterface object, noting that the getters of the storage class will apply to all
+   *     supported interfaces.
+   *     - The getter method names should not clash in name with any other interface getter
+   *       specializations if those other interfaces are meant to be aggregated in the same
+   *       NodeInterfaces object.
+   *
+   * \param[in] node Node-like object from which to get the node interfaces
+   */
+  template<typename NodeT>
+  NodeInterfaces(NodeT & node)  // NOLINT(runtime/explicit)
+  : NodeInterfacesSupportsT(node)
+  {}
+
+  /// NodeT::SharedPtr Constructor
+  template<typename NodeT>
+  NodeInterfaces(std::shared_ptr<NodeT> node)  // NOLINT(runtime/explicit)
+  : NodeInterfaces(
+      [&]() -> NodeT & {
+        if (!node) {
+          throw std::invalid_argument("given node pointer is nullptr");
+        }
+        return *node;
+      }())
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp
@@ -19,6 +19,7 @@

 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
@@ -103,25 +103,6 @@ public:
  virtual
  ~NodeParameters();

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

 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"

@@ -45,17 +46,6 @@ struct OnSetParametersCallbackHandle
  OnParametersSetCallbackType callback;
 };

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

-  /// Declare a parameter.
-  /**
-   * \sa rclcpp::Node::declare_parameter
-   */
-  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
-  virtual
-  const rclcpp::ParameterValue &
-  declare_parameter(const std::string & name) = 0;
-
  /// Declare and initialize a parameter.
  /**
   * \sa rclcpp::Node::declare_parameter
@@ -235,4 +216,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp
@@ -20,6 +20,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/visibility_control.hpp"

@@ -62,4 +63,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp
@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"

 namespace rclcpp
@@ -37,4 +38,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp
@@ -17,6 +17,7 @@

 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"

@@ -47,4 +48,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp
+++ b/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
--- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp
@@ -24,6 +24,7 @@

 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
@@ -97,4 +98,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp
@@ -17,6 +17,7 @@

 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"

@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp

+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
--- a/rclcpp/include/rclcpp/node_options.hpp
+++ b/rclcpp/include/rclcpp/node_options.hpp
@@ -42,6 +42,7 @@ public:
   *   - arguments = {}
   *   - parameter_overrides = {}
   *   - use_global_arguments = true
+   *   - enable_rosout = true
   *   - use_intra_process_comms = false
   *   - enable_topic_statistics = false
   *   - start_parameter_services = true
--- a/rclcpp/include/rclcpp/parameter.hpp
+++ b/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

--- a/rclcpp/include/rclcpp/parameter_client.hpp
+++ b/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)
--- a/rclcpp/include/rclcpp/parameter_event_handler.hpp
+++ b/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)))
--- a/rclcpp/include/rclcpp/parameter_events_filter.hpp
+++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp
@@ -45,6 +45,7 @@ public:
   * \param[in] names A list of parameter names of interest.
   * \param[in] types A list of the types of parameter events of iterest.
   *    EventType NEW, DELETED, or CHANGED
+   * \throws std::invalid_argument if event is NULL.
   *
   * Example Usage:
   *
--- a/rclcpp/include/rclcpp/parameter_map.hpp
+++ b/rclcpp/include/rclcpp/parameter_map.hpp
@@ -41,6 +41,16 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);

+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
+
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/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>;
@@ -176,8 +177,7 @@ public:
    const rclcpp::QoS & qos,
    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
  {
-    // Topic is unused for now.
-    (void)topic;
+    (void)qos;
    (void)options;

    // If needed, setup intra process communication.
@@ -185,19 +185,23 @@ public:
      auto context = node_base->get_context();
      // Get the intra process manager instance for this context.
      auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
-      // Register the publisher with the intra process manager.
-      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
+      // Check if the QoS is compatible with intra-process.
+      auto qos_profile = get_actual_qos();
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
        throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic +
+                "' allowed only with keep last history qos policy");
      }
-      if (qos.depth() == 0) {
+      if (qos_profile.depth() == 0) {
        throw std::invalid_argument(
-                "intraprocess communication is not allowed with a zero qos history depth value");
+                "intraprocess communication on topic '" + topic +
+                "' is not allowed with a zero qos history depth value");
      }
-      if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) {
+      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
        throw std::invalid_argument(
                "intraprocess communication allowed only with volatile durability");
      }
+      // Register the publisher with the intra process manager.
      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
      this->setup_intra_process(
        intra_process_publisher_id,
@@ -263,10 +267,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 +324,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 +366,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 +374,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 +505,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 +516,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 +553,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 +579,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
--- a/rclcpp/include/rclcpp/publisher_base.hpp
+++ b/rclcpp/include/rclcpp/publisher_base.hpp
@@ -18,11 +18,14 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>

+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
 #include <sstream>
 #include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>

 #include "rcl/publisher.h"
@@ -33,6 +36,7 @@
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/time.hpp"

 namespace rclcpp
 {
@@ -110,9 +114,10 @@ public:
  get_publisher_handle() const;

  /// Get all the QoS event handlers associated with this publisher.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
  RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
  get_event_handlers() const;

  /// Get subscription count
@@ -203,6 +208,111 @@ public:
  std::vector<rclcpp::NetworkFlowEndpoint>
  get_network_flow_endpoints() const;

+  /// Wait until all published messages are acknowledged or until the specified timeout elapses.
+  /**
+   * This method waits until all published messages are acknowledged by all matching
+   * subscriptions or the given timeout elapses.
+   *
+   * If the timeout is negative then this method will block indefinitely until all published
+   * messages are acknowledged.
+   * If the timeout is zero then this method will not block, it will check if all published
+   * messages are acknowledged and return immediately.
+   * If the timeout is greater than zero, this method will wait until all published messages are
+   * acknowledged or the timeout elapses.
+   *
+   * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE.
+   * Otherwise this method will immediately return `true`.
+   *
+   * \param[in] timeout the duration to wait for all published messages to be acknowledged.
+   * \return `true` if all published messages were acknowledged before the given timeout
+   *   elapsed, otherwise `false`.
+   * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs
+   * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or
+   *   less than std::chrono::nanoseconds::min()
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli>
+  bool
+  wait_for_all_acked(
+    std::chrono::duration<DurationRepT, DurationT> timeout =
+    std::chrono::duration<DurationRepT, DurationT>(-1)) const
+  {
+    rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count();
+
+    rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout);
+    if (ret == RCL_RET_OK) {
+      return true;
+    } else if (ret == RCL_RET_TIMEOUT) {
+      return false;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered publisher event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
 protected:
  template<typename EventCallbackT>
  void
@@ -216,7 +326,7 @@ protected:
      rcl_publisher_event_init,
      publisher_handle_,
      event_type);
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
  }

  RCLCPP_PUBLIC
@@ -226,7 +336,8 @@ protected:

  std::shared_ptr<rcl_publisher_t> publisher_handle_;

-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_publisher_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;

  using IntraProcessManagerWeakPtr =
    std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
@@ -235,6 +346,8 @@ protected:
  uint64_t intra_process_publisher_id_;

  rmw_gid_t rmw_gid_;
+
+  const rosidl_message_type_support_t type_support_;
 };

 }  // namespace rclcpp
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
  /// Optional custom allocator.
  std::shared_ptr<Allocator> allocator = nullptr;

-  PublisherOptionsWithAllocator<Allocator>() {}
+  PublisherOptionsWithAllocator() {}

  /// Constructor using base class as input.
  explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)
@@ -119,6 +119,10 @@ private:
  rcl_allocator_t
  get_rcl_allocator() const
  {
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    }
+
    if (!plain_allocator_storage_) {
      plain_allocator_storage_ =
        std::make_shared<PlainAllocator>(*this->get_allocator());
--- a/rclcpp/include/rclcpp/qos_event.hpp
+++ b/rclcpp/include/rclcpp/qos_event.hpp
@@ -17,16 +17,21 @@

 #include <functional>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>

 #include "rcl/error_handling.h"
+#include "rcl/event_callback.h"
+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/incompatible_qos_events_statuses.h"

 #include "rcutils/logging_macros.h"

+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/waitable.hpp"

 namespace rclcpp
@@ -84,6 +89,11 @@ public:
 class QOSEventHandlerBase : public Waitable
 {
 public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
  RCLCPP_PUBLIC
  virtual ~QOSEventHandlerBase();

@@ -102,9 +112,111 @@ public:
  bool
  is_ready(rcl_wait_set_t * wait_set) override;

+  /// Set a callback to be called when each new event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_event_set_callback
+   * \sa rcl_event_set_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   */
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_ready_callback "
+              "is not callable.");
+    }
+
+    // Note: we bind the int identifier argument to this waitable's entity types
+    auto new_callback =
+      [callback, this](size_t number_of_events) {
+        try {
+          callback(number_of_events, static_cast<int>(EntityType::Event));
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            // TODO(wjwwood): get this class access to the node logger it is associated with
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on ready' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            rclcpp::get_logger("rclcpp"),
+            "rclcpp::QOSEventHandlerBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on ready' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_event_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_event_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_event_callback_));
+  }
+
+  /// Unset the callback registered for new events, if any.
+  void
+  clear_on_ready_callback() override
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    if (on_new_event_callback_) {
+      set_on_new_event_callback(nullptr, nullptr);
+      on_new_event_callback_ = nullptr;
+    }
+  }
+
 protected:
+  RCLCPP_PUBLIC
+  void
+  set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
+
  rcl_event_t event_handle_;
  size_t wait_set_event_index_;
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
 };

 template<typename EventCallbackT, typename ParentHandleT>
@@ -117,9 +229,8 @@ public:
    InitFuncT init_func,
    ParentHandleT parent_handle,
    EventTypeEnum event_type)
-  : event_callback_(callback)
+  : parent_handle_(parent_handle), event_callback_(callback)
  {
-    parent_handle_ = parent_handle;
    event_handle_ = rcl_get_zero_initialized_event();
    rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
    if (ret != RCL_RET_OK) {
@@ -148,6 +259,13 @@ public:
    return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
  }

+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override
+  {
+    (void)id;
+    return take_data();
+  }
+
  /// Execute any entities of the Waitable that are ready.
  void
  execute(std::shared_ptr<void> & data) override
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/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>
@@ -348,6 +481,14 @@ public:
  {
    rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);

+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        node_logger_.get_child("rclcpp"),
+        "failed to send response to %s (timeout): %s",
+        this->get_service_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
    if (ret != RCL_RET_OK) {
      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
    }
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -164,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;
        });
    }

@@ -204,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",
@@ -213,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",
@@ -222,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",
@@ -231,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",
@@ -244,7 +236,7 @@ public:
      detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition);
    }

-    for (auto waitable : waitable_handles_) {
+    for (const std::shared_ptr<Waitable> & waitable : waitable_handles_) {
      waitable->add_to_wait_set(wait_set);
    }
    return true;
@@ -402,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);
@@ -432,13 +424,17 @@ public:

  rcl_allocator_t get_allocator() override
  {
-    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    }
  }

  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;
@@ -447,7 +443,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;
@@ -456,7 +452,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;
@@ -465,7 +461,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;
@@ -474,7 +470,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;
@@ -483,7 +479,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;
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -186,17 +186,27 @@ public:
      auto qos_profile = get_actual_qos();
      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
        throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' allowed only with keep last history qos policy");
      }
      if (qos_profile.depth() == 0) {
        throw std::invalid_argument(
-                "intraprocess communication is not allowed with 0 depth qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' is not allowed with 0 depth qos policy");
      }
      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
        throw std::invalid_argument(
                "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>(
@@ -276,7 +286,7 @@ public:

  /// Take the next message from the inter-process subscription.
  /**
-   * This verison takes a SubscribedType which is different frmo the
+   * This version takes a SubscribedType which is different from the
   * ROSMessageType when a rclcpp::TypeAdapter is in used.
   *
   * \sa take(ROSMessageType &, rclcpp::MessageInfo &)
@@ -355,11 +365,31 @@ public:
    void * loaned_message,
    const rclcpp::MessageInfo & message_info) override
  {
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
+    }
+
    auto typed_message = static_cast<ROSMessageType *>(loaned_message);
    // message is loaned, so we have to make sure that the deleter does not deallocate the message
    auto sptr = std::shared_ptr<ROSMessageType>(
      typed_message, [](ROSMessageType * msg) {(void) msg;});
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
    any_callback_.dispatch(sptr, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
  }

  /// Return the borrowed message.
@@ -404,13 +434,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
--- a/rclcpp/include/rclcpp/subscription_base.hpp
+++ b/rclcpp/include/rclcpp/subscription_base.hpp
@@ -17,16 +17,20 @@

 #include <atomic>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
 #include <utility>

+#include "rcl/event_callback.h"
 #include "rcl/subscription.h"

+#include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"

 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
@@ -35,6 +39,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"

@@ -99,9 +104,10 @@ public:
  get_subscription_handle() const;

  /// Get all the QoS event handlers associated with this subscription.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
  RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
  get_event_handlers() const;

  /// Get the actual QoS settings, after the defaults have been determined.
@@ -283,6 +289,243 @@ public:
  std::vector<rclcpp::NetworkFlowEndpoint>
  get_network_flow_endpoints() const;

+  /// Set a callback to be called when each new message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rmw_subscription_set_on_new_message_callback
+   * \sa rcl_subscription_set_on_new_message_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_message_callback "
+              "is not callable.");
+    }
+
+    auto new_callback =
+      [callback, this](size_t number_of_messages) {
+        try {
+          callback(number_of_messages);
+        } catch (const std::exception & exception) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught " << rmw::impl::cpp::demangle(exception) <<
+              " exception in user-provided callback for the 'on new message' callback: " <<
+              exception.what());
+        } catch (...) {
+          RCLCPP_ERROR_STREAM(
+            node_logger_,
+            "rclcpp::SubscriptionBase@" << this <<
+              " caught unhandled exception in user-provided callback " <<
+              "for the 'on new message' callback");
+        }
+      };
+
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    // Set it temporarily to the new callback, while we replace the old one.
+    // This two-step setting, prevents a gap where the old std::function has
+    // been replaced but the middleware hasn't been told about the new one yet.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&new_callback));
+
+    // Store the std::function to keep it in scope, also overwrites the existing one.
+    on_new_message_callback_ = new_callback;
+
+    // Set it again, now using the permanent storage.
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      static_cast<const void *>(&on_new_message_callback_));
+  }
+
+  /// Unset the callback registered for new messages, if any.
+  void
+  clear_on_new_message_callback()
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+    if (on_new_message_callback_) {
+      set_on_new_message_callback(nullptr, nullptr);
+      on_new_message_callback_ = nullptr;
+    }
+  }
+
+  /// Set a callback to be called when each new intra-process message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_intra_process_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_intra_process_message_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    subscription_intra_process_->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new intra-process messages, if any.
+  void
+  clear_on_new_intra_process_message_callback()
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    subscription_intra_process_->clear_on_ready_callback();
+  }
+
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered subscription event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
+  /// Check if content filtered topic feature of the subscription instance is enabled.
+  /**
+   * \return boolean flag indicating if the content filtered topic of this subscription is enabled.
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_cft_enabled() const;
+
+  /// Set the filter expression and expression parameters for the subscription.
+  /**
+   * \param[in] filter_expression A filter expression to set.
+   *   \sa ContentFilterOptions::filter_expression
+   *   An empty string ("") will clear the content filter setting of the subscription.
+   * \param[in] expression_parameters Array of expression parameters to set.
+   *   \sa ContentFilterOptions::expression_parameters
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  void
+  set_content_filter(
+    const std::string & filter_expression,
+    const std::vector<std::string> & expression_parameters = {});
+
+  /// Get the filter expression and expression parameters for the subscription.
+  /**
+   * \return rclcpp::ContentFilterOptions The content filter options to get.
+   * \throws RCLBadAlloc if memory cannot be allocated
+   * \throws RCLError if an unexpect error occurs
+   */
+  RCLCPP_PUBLIC
+  rclcpp::ContentFilterOptions
+  get_content_filter() const;
+
 protected:
  template<typename EventCallbackT>
  void
@@ -297,7 +540,7 @@ protected:
      get_subscription_handle(),
      event_type);
    qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
  }

  RCLCPP_PUBLIC
@@ -307,17 +550,24 @@ protected:
  bool
  matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;

+  RCLCPP_PUBLIC
+  void
+  set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data);
+
  rclcpp::node_interfaces::NodeBaseInterface * const node_base_;

  std::shared_ptr<rcl_node_t> node_handle_;
  std::shared_ptr<rcl_subscription_t> subscription_handle_;
  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  rclcpp::Logger node_logger_;

-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_subscription_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;

  bool use_intra_process_;
  IntraProcessManagerWeakPtr weak_ipm_;
  uint64_t intra_process_subscription_id_;
+  std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;

 private:
  RCLCPP_DISABLE_COPY(SubscriptionBase)
@@ -329,6 +579,9 @@ private:
  std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
  std::unordered_map<rclcpp::QOSEventHandlerBase *,
    std::atomic<bool>> qos_events_in_use_by_wait_set_;
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
 };

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

@@ -81,6 +82,8 @@ struct SubscriptionOptionsBase
  TopicStatisticsOptions topic_stats_options;

  QosOverridingOptions qos_overriding_options;
+
+  ContentFilterOptions content_filter_options;
 };

 /// Structure containing optional configuration for Subscriptions.
@@ -94,7 +97,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
  /// Optional custom allocator.
  std::shared_ptr<Allocator> allocator = nullptr;

-  SubscriptionOptionsWithAllocator<Allocator>() {}
+  SubscriptionOptionsWithAllocator() {}

  /// Constructor using base class as input.
  explicit SubscriptionOptionsWithAllocator(
@@ -123,6 +126,21 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
      rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options);
    }

+    // Copy content_filter_options into rcl_subscription_options.
+    if (!content_filter_options.filter_expression.empty()) {
+      std::vector<const char *> cstrings =
+        get_c_vector_string(content_filter_options.expression_parameters);
+      rcl_ret_t ret = rcl_subscription_options_set_content_filter_options(
+        get_c_string(content_filter_options.filter_expression),
+        cstrings.size(),
+        cstrings.data(),
+        &result);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "failed to set content_filter_options");
+      }
+    }
+
    return result;
  }

@@ -145,11 +163,15 @@ private:
  rcl_allocator_t
  get_rcl_allocator() const
  {
-    if (!plain_allocator_storage_) {
-      plain_allocator_storage_ =
-        std::make_shared<PlainAllocator>(*this->get_allocator());
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      if (!plain_allocator_storage_) {
+        plain_allocator_storage_ =
+          std::make_shared<PlainAllocator>(*this->get_allocator());
+      }
+      return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
    }
-    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
  }

  // This is a temporal workaround, to make sure that get_allocator()
--- a/rclcpp/include/rclcpp/time_source.hpp
+++ b/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_;

--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/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
--- a/rclcpp/include/rclcpp/utilities.hpp
+++ b/rclcpp/include/rclcpp/utilities.hpp
@@ -65,13 +65,16 @@ enum class SignalHandlerOptions
 *
 * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
 *
- * \param signal_handler_options option to indicate which signal handlers should be installed.
+ * \param[in] argc number of command-line arguments to parse.
+ * \param[in] argv array of command-line arguments to parse.
+ * \param[in] init_options initialization options to apply.
+ * \param[in] signal_handler_options option to indicate which signal handlers should be installed.
 */
 RCLCPP_PUBLIC
 void
 init(
  int argc,
-  char const * const argv[],
+  char const * const * argv,
  const InitOptions & init_options = InitOptions(),
  SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All);

@@ -87,7 +90,7 @@ init(
 *
 * This function is thread-safe.
 *
- * \param signal_handler_options option to indicate which signal handlers should be installed.
+ * \param[in] signal_handler_options option to indicate which signal handlers should be installed.
 * \return true if signal handler was installed by this function, false if already installed.
 */
 RCLCPP_PUBLIC
@@ -136,7 +139,7 @@ RCLCPP_PUBLIC
 std::vector<std::string>
 init_and_remove_ros_arguments(
  int argc,
-  char const * const argv[],
+  char const * const * argv,
  const InitOptions & init_options = InitOptions());

 /// Remove ROS-specific arguments from argument vector.
@@ -154,7 +157,7 @@ init_and_remove_ros_arguments(
 */
 RCLCPP_PUBLIC
 std::vector<std::string>
-remove_ros_arguments(int argc, char const * const argv[]);
+remove_ros_arguments(int argc, char const * const * argv);

 /// Check rclcpp's status.
 /**
@@ -318,6 +321,15 @@ RCLCPP_PUBLIC
 const char *
 get_c_string(const std::string & string_in);

+/// Return the std::vector of C string from the given std::vector<std::string>.
+/**
+ * \param[in] strings_in is a std::vector of std::string
+ * \return the std::vector of C string from the std::vector<std::string>
+ */
+RCLCPP_PUBLIC
+std::vector<const char *>
+get_c_vector_string(const std::vector<std::string> & strings_in);
+
 }  // namespace rclcpp

 #endif  // RCLCPP__UTILITIES_HPP_
--- a/rclcpp/include/rclcpp/wait_for_message.hpp
+++ b/rclcpp/include/rclcpp/wait_for_message.hpp
@@ -15,12 +15,16 @@
 #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
 #define RCLCPP__WAIT_FOR_MESSAGE_HPP_

+#include <future>
 #include <memory>
 #include <string>

+#include "rcpputils/scope_exit.hpp"
+
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/wait_set.hpp"
+#include "rclcpp/qos.hpp"

 namespace rclcpp
 {
@@ -52,8 +56,9 @@ bool wait_for_message(
      }
    });

-  rclcpp::WaitSet wait_set;
+  rclcpp::WaitSet wait_set({}, {}, {}, {}, {}, {}, context);
  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) {
@@ -76,10 +81,11 @@ bool wait_for_message(
 /**
 * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
 *
- * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[out] out is the message to be filled when a new message is arriving
 * \param[in] node the node pointer to initialize the subscription on.
 * \param[in] topic the topic to wait for messages.
 * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \param[in] qos parameter specifying QoS settings for the subscription.
 * \return true if a message was successfully received, false if message could not
 * be obtained or shutdown was triggered asynchronously on the context.
 */
@@ -88,9 +94,10 @@ bool wait_for_message(
  MsgT & out,
  rclcpp::Node::SharedPtr node,
  const std::string & topic,
-  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1),
+  const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS())
 {
-  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<const MsgT>) {});
+  auto sub = node->create_subscription<MsgT>(topic, qos, [](const std::shared_ptr<const MsgT>) {});
  return wait_for_message<MsgT, Rep, Period>(
    out, sub, node->get_node_options().context(), time_to_wait);
 }
--- a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp
+++ b/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.
--- a/rclcpp/include/rclcpp/wait_set_template.hpp
+++ b/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));
--- a/rclcpp/include/rclcpp/waitable.hpp
+++ b/rclcpp/include/rclcpp/waitable.hpp
@@ -16,6 +16,7 @@
 #define RCLCPP__WAITABLE_HPP_

 #include <atomic>
+#include <functional>
 #include <memory>

 #include "rclcpp/macros.hpp"
@@ -159,6 +160,24 @@ public:
  std::shared_ptr<void>
  take_data() = 0;

+  /// Take the data so that it can be consumed with `execute`.
+  /**
+   * This function allows to specify an entity ID to take the data from.
+   * Entity IDs are identifiers that can be defined by waitable-derived
+   * classes that are composed of several distinct entities.
+   * The main use-case is in conjunction with the listener APIs.
+   *
+   * \param[in] id the id of the entity from which to take
+   * \returns the type-erased data taken from entity specified
+   *
+   * \sa rclcpp::Waitable::take_data
+   * \sa rclcpp::Waitable::set_on_ready_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id);
+
  /// Execute data that is passed in.
  /**
   * Before calling this method, the Waitable should be added to a wait set,
@@ -200,6 +219,45 @@ public:
  bool
  exchange_in_use_by_wait_set_state(bool in_use_state);

+  /// Set a callback to be called whenever the waitable becomes ready.
+  /**
+   * The callback receives a size_t which is the number of times the waitable was ready
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if waitable was triggered before any
+   * callback was set.
+   *
+   * The callback also receives an int identifier argument.
+   * This is needed because a Waitable may be composed of several distinct entities,
+   * such as subscriptions, services, etc.
+   * The application should provide a generic callback function that will be then
+   * forwarded by the waitable to all of its entities.
+   * Before forwarding, a different value for the identifier argument will be
+   * bond to the function.
+   * This implies that the provided callback can use the identifier to behave
+   * differently depending on which entity triggered the waitable to become ready.
+   *
+   * Note: this function must be overridden with a proper implementation
+   * by the custom classes who inherit from rclcpp::Waitable if they want to use it.
+   *
+   * \sa rclcpp::Waitable::clear_on_ready_callback
+   *
+   * \param[in] callback functor to be called when the waitable becomes ready
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback);
+
+  /// Unset any callback registered via set_on_ready_callback.
+  /**
+   * Note: this function must be overridden with a proper implementation
+   * by the custom classes who inherit from rclcpp::Waitable if they want to use it.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  clear_on_ready_callback();
+
 private:
  std::atomic<bool> in_use_by_wait_set_{false};
 };  // class Waitable
--- a/rclcpp/package.xml
+++ b/rclcpp/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
  <name>rclcpp</name>
-  <version>14.0.0</version>
+  <version>16.0.18</version>
  <description>The ROS client library in C++.</description>
  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
@@ -39,6 +39,7 @@
  <depend>tracetools</depend>

  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
--- a/rclcpp/resource/logging.hpp.em
+++ b/rclcpp/resource/logging.hpp.em
@@ -125,8 +125,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
  do { \
    static_assert( \
-      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
-      typename ::rclcpp::Logger>::value, \
+      ::std::is_convertible_v<decltype(logger), ::rclcpp::Logger>, \
      "First argument to logging macros must be an rclcpp::Logger"); \
@[ if 'throttle' in feature_combination]@ \
    auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
@@ -141,8 +140,8 @@ def get_rclcpp_suffix_from_features(features):
    }; \
@[ end if] \
@[ if 'stream' in feature_combination]@
-    std::stringstream ss; \
-    ss << @(stream_arg); \
+    std::stringstream rclcpp_stream_ss_; \
+    rclcpp_stream_ss_ << @(stream_arg); \
@[ end if]@
    RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \
@{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@
@@ -153,7 +152,7 @@ def get_rclcpp_suffix_from_features(features):
@[ if 'stream' not in feature_combination]@
      __VA_ARGS__); \
@[ else]@
-      "%s", ss.str().c_str()); \
+      "%s", rclcpp_stream_ss_.str().c_str()); \
@[ end if]@
  } while (0)

--- a/rclcpp/src/rclcpp/callback_group.cpp
+++ b/rclcpp/src/rclcpp/callback_group.cpp
@@ -12,9 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

-#include "rclcpp/callback_group.hpp"
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>

-#include <vector>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/waitable.hpp"

 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
@@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup(
  automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}

+CallbackGroup::~CallbackGroup()
+{
+  trigger_notify_guard_condition();
+}

 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -40,6 +54,51 @@ CallbackGroup::type() const
  return type_;
 }

+void CallbackGroup::collect_all_ptrs(
+  std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
+  std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
+  std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
+  std::function<void(const rclcpp::TimerBase::SharedPtr &)> timer_func,
+  std::function<void(const rclcpp::Waitable::SharedPtr &)> waitable_func) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const rclcpp::SubscriptionBase::WeakPtr & weak_ptr : subscription_ptrs_) {
+    rclcpp::SubscriptionBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      sub_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::ServiceBase::WeakPtr & weak_ptr : service_ptrs_) {
+    rclcpp::ServiceBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      service_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::ClientBase::WeakPtr & weak_ptr : client_ptrs_) {
+    rclcpp::ClientBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      client_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::TimerBase::WeakPtr & weak_ptr : timer_ptrs_) {
+    rclcpp::TimerBase::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      timer_func(ref_ptr);
+    }
+  }
+
+  for (const rclcpp::Waitable::WeakPtr & weak_ptr : waitable_ptrs_) {
+    rclcpp::Waitable::SharedPtr ref_ptr = weak_ptr.lock();
+    if (ref_ptr) {
+      waitable_func(ref_ptr);
+    }
+  }
+}
+
 std::atomic_bool &
 CallbackGroup::get_associated_with_executor_atomic()
 {
@@ -52,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const
  return automatically_add_to_executor_with_node_;
 }

+rclcpp::GuardCondition::SharedPtr
+CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
+    if (associated_with_executor_) {
+      trigger_notify_guard_condition();
+    }
+    notify_guard_condition_ = nullptr;
+  }
+
+  if (!notify_guard_condition_) {
+    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
+  }
+
+  return notify_guard_condition_;
+}
+
+void
+CallbackGroup::trigger_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_) {
+    notify_guard_condition_->trigger();
+  }
+}
+
 void
 CallbackGroup::add_subscription(
  const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)
--- a/rclcpp/src/rclcpp/client.cpp
+++ b/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");
+  }
+}
--- a/rclcpp/src/rclcpp/clock.cpp
+++ b/rclcpp/src/rclcpp/clock.cpp
@@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
  return sleep_until(now() + rel_time, context);
 }

+bool
+Clock::started()
+{
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock is not rcl_clock_valid");
+  }
+  return rcl_clock_time_started(get_clock_handle());
+}
+
+bool
+Clock::wait_until_started(Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  if (started()) {
+    return true;
+  } else {
+    // Wait until the first non-zero time
+    return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
+  }
+}
+
+bool
+Clock::wait_until_started(
+  const Duration & timeout,
+  Context::SharedPtr context,
+  const Duration & wait_tick_ns)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  Clock timeout_clock = Clock(RCL_STEADY_TIME);
+  Time start = timeout_clock.now();
+
+  // Check if the clock has started every wait_tick_ns nanoseconds
+  // Context check checks for rclcpp::shutdown()
+  while (!started() && context->is_valid()) {
+    if (timeout < wait_tick_ns) {
+      timeout_clock.sleep_for(timeout);
+    } else {
+      Duration time_left = start + timeout - timeout_clock.now();
+      if (time_left > wait_tick_ns) {
+        timeout_clock.sleep_for(Duration(wait_tick_ns));
+      } else {
+        timeout_clock.sleep_for(time_left);
+      }
+    }
+
+    if (timeout_clock.now() - start > timeout) {
+      return started();
+    }
+  }
+  return started();
+}
+
+
 bool
 Clock::ros_time_is_active()
 {
--- a/rclcpp/src/rclcpp/context.cpp
+++ b/rclcpp/src/rclcpp/context.cpp
@@ -32,8 +32,6 @@
 #include "rcutils/error_handling.h"
 #include "rcutils/macros.h"

-#include "rmw/impl/cpp/demangle.hpp"
-
 #include "./logging_mutex.hpp"

 using rclcpp::Context;
@@ -192,7 +190,7 @@ __delete_context(rcl_context_t * context)
 void
 Context::init(
  int argc,
-  char const * const argv[],
+  char const * const * argv,
  const rclcpp::InitOptions & init_options)
 {
  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
@@ -219,7 +217,7 @@ Context::init(
    if (0u == count) {
      ret = rcl_logging_configure_with_output_handler(
        &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
+        rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
        rclcpp_logging_output_handler);
      if (RCL_RET_OK != ret) {
        rcl_context_.reset();
@@ -499,7 +497,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
      std::unique_lock<std::mutex> lock(interrupt_mutex_);
      auto start = std::chrono::steady_clock::now();
      // this will release the lock while waiting
-      interrupt_condition_variable_.wait_for(lock, nanoseconds);
+      interrupt_condition_variable_.wait_for(lock, time_left);
      time_left -= std::chrono::steady_clock::now() - start;
    }
  } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());
--- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp
+++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.

-#include "./resolve_parameter_overrides.hpp"
+#include "resolve_parameter_overrides.hpp"

 #include <string>
 #include <map>
@@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides(
        [params]() {
          rcl_yaml_node_struct_fini(params);
        });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());

-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            result[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
+      if (initial_map.count(node_fqn) > 0) {
+        // Combine parameter yaml files, overwriting values in older ones
+        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
+          result[param.get_name()] =
+            rclcpp::ParameterValue(param.get_value_message());
        }
      }
    }
--- a/rclcpp/src/rclcpp/detail/utilities.cpp
+++ b/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)
 {
--- a/rclcpp/src/rclcpp/duration.cpp
+++ b/rclcpp/src/rclcpp/duration.cpp
@@ -37,11 +37,6 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
  rcl_duration_.nanoseconds += nanoseconds;
 }

-Duration::Duration(rcl_duration_value_t nanoseconds)
-{
-  rcl_duration_.nanoseconds = nanoseconds;
-}
-
 Duration::Duration(std::chrono::nanoseconds nanoseconds)
 {
  rcl_duration_.nanoseconds = nanoseconds.count();
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -106,11 +106,11 @@ Executor::~Executor()
  weak_groups_associated_with_executor_to_nodes_.clear();
  weak_groups_to_nodes_associated_with_executor_.clear();
  weak_groups_to_nodes_.clear();
-  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
    auto guard_condition = pair.second;
    memory_strategy_->remove_guard_condition(guard_condition);
  }
-  weak_nodes_to_guard_conditions_.clear();
+  weak_groups_to_guard_conditions_.clear();

  // Finalize the wait set.
  if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -204,8 +204,7 @@ Executor::add_callback_group_to_map(
  if (has_executor.exchange(true)) {
    throw std::runtime_error("Callback group has already been added to an executor.");
  }
-  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+
  rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
  auto insert_info =
    weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -215,21 +214,24 @@ Executor::add_callback_group_to_map(
  }
  // Also add to the map that contains all callback groups
  weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-  if (is_new_node) {
-    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
-      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());
-      }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
+    // Add the callback_group's notify condition to the guard condition handles
+    memory_strategy_->add_guard_condition(*callback_group_guard_condition);
+  }
+
+  if (notify) {
+    // Interrupt waiting to handle new node
+    try {
+      interrupt_guard_condition_.trigger();
+    } catch (const rclcpp::exceptions::RCLError & ex) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to trigger guard condition on callback group add: ") + ex.what());
    }
-    // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(gc);
  }
 }

@@ -300,7 +302,12 @@ 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_))
  {
-    weak_nodes_to_guard_conditions_.erase(node_ptr);
+    auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (iter != weak_groups_to_guard_conditions_.end()) {
+      memory_strategy_->remove_guard_condition(iter->second);
+    }
+    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
    if (notify) {
      try {
        interrupt_guard_condition_.trigger();
@@ -310,7 +317,6 @@ Executor::remove_callback_group_from_map(
                  "Failed to trigger guard condition on callback group remove: ") + ex.what());
      }
    }
-    memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
  }
 }

@@ -414,8 +420,8 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration)

 void Executor::spin_all(std::chrono::nanoseconds max_duration)
 {
-  if (max_duration <= 0ns) {
-    throw std::invalid_argument("max_duration must be positive");
+  if (max_duration < 0ns) {
+    throw std::invalid_argument("max_duration must be greater than or equal to 0");
  }
  return this->spin_some_impl(max_duration, true);
 }
@@ -640,6 +646,11 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
      subscription->get_topic_name(),
      [&]() {return subscription->take_type_erased(message.get(), message_info);},
      [&]() {subscription->handle_message(message, message_info);});
+    // TODO(clalancette): In the case that the user is using the MessageMemoryPool,
+    // and they take a shared_ptr reference to the message in the callback, this can
+    // inadvertently return the message to the pool when the user is still using it.
+    // This is a bug that needs to be fixed in the pool, and we should probably have
+    // a custom deleter for the message that actually does the return_message().
    subscription->return_message(message);
  }
 }
@@ -700,12 +711,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
        auto weak_node_ptr = pair.second;
        if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
          invalid_group_ptrs.push_back(weak_group_ptr);
-          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
-            auto guard_condition = node_guard_pair->second;
-            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-            memory_strategy_->remove_guard_condition(guard_condition);
-          }
        }
      }
      std::for_each(
@@ -721,6 +726,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
          {
            weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
          }
+          auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr);
+          if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) {
+            auto guard_condition = callback_guard_pair->second;
+            weak_groups_to_guard_conditions_.erase(group_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
          weak_groups_to_nodes_.erase(group_ptr);
        });
    }
@@ -928,3 +939,9 @@ Executor::has_node(
      return other_ptr == node_ptr;
    }) != weak_groups_to_nodes.end();
 }
+
+bool
+Executor::is_spinning()
+{
+  return spinning;
+}
--- a/rclcpp/src/rclcpp/executors.cpp
+++ b/rclcpp/src/rclcpp/executors.cpp
@@ -17,7 +17,9 @@
 void
 rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
  exec.spin_node_some(node_ptr);
 }

@@ -30,7 +32,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr)
 void
 rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
  exec.add_node(node_ptr);
  exec.spin();
  exec.remove_node(node_ptr);
--- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
+++ b/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;
    {
--- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
+++ b/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);
 }
--- a/rclcpp/src/rclcpp/generic_publisher.cpp
+++ b/rclcpp/src/rclcpp/generic_publisher.cpp
@@ -31,4 +31,49 @@ void GenericPublisher::publish(const rclcpp::SerializedMessage & message)
  }
 }

+void GenericPublisher::publish_as_loaned_msg(const rclcpp::SerializedMessage & message)
+{
+  auto loaned_message = borrow_loaned_message();
+  deserialize_message(message.get_rcl_serialized_message(), loaned_message);
+  publish_loaned_message(loaned_message);
+}
+
+void * GenericPublisher::borrow_loaned_message()
+{
+  void * loaned_message = nullptr;
+  auto return_code = rcl_borrow_loaned_message(
+    get_publisher_handle().get(), &type_support_, &loaned_message);
+
+  if (return_code != RMW_RET_OK) {
+    if (return_code == RCL_RET_UNSUPPORTED) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        return_code,
+        "current middleware cannot support loan messages");
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to borrow loaned msg");
+    }
+  }
+  return loaned_message;
+}
+
+void GenericPublisher::deserialize_message(
+  const rmw_serialized_message_t & serialized_message,
+  void * deserialized_msg)
+{
+  auto return_code = rmw_deserialize(&serialized_message, &type_support_, deserialized_msg);
+  if (return_code != RMW_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to deserialize msg");
+  }
+}
+
+void GenericPublisher::publish_loaned_message(void * loaned_message)
+{
+  auto return_code = rcl_publish_loaned_message(
+    get_publisher_handle().get(), loaned_message, NULL);
+
+  if (return_code != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish loaned message");
+  }
+}
+
 }  // namespace rclcpp
--- a/rclcpp/src/rclcpp/guard_condition.cpp
+++ b/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"
@@ -72,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_++;
  }
 }

@@ -84,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
--- a/rclcpp/src/rclcpp/logging_mutex.cpp
+++ b/rclcpp/src/rclcpp/logging_mutex.cpp
@@ -14,6 +14,7 @@

 #include <memory>
 #include <mutex>
+#include <stdexcept>

 #include "rcutils/macros.h"

--- a/rclcpp/src/rclcpp/memory_strategy.cpp
+++ b/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) {
--- a/rclcpp/src/rclcpp/node.cpp
+++ b/rclcpp/src/rclcpp/node.cpp
@@ -310,24 +310,6 @@ Node::create_callback_group(
  return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }

-const rclcpp::ParameterValue &
-Node::declare_parameter(const std::string & name)
-{
-#ifndef _WIN32
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  return this->node_parameters_->declare_parameter(name);
-#ifndef _WIN32
-# pragma GCC diagnostic pop
-#else
-# pragma warning(pop)
-#endif
-}
-
 const rclcpp::ParameterValue &
 Node::declare_parameter(
  const std::string & name,
--- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
@@ -236,6 +236,9 @@ __check_parameters(
    } else {
      descriptor = parameter_infos[name].descriptor;
    }
+    if (descriptor.name.empty()) {
+      descriptor.name = name;
+    }
    const auto new_type = parameter.get_type();
    const auto specified_type = static_cast<rclcpp::ParameterType>(descriptor.type);
    result.successful = descriptor.dynamic_typing || specified_type == new_type;
@@ -464,14 +467,6 @@ declare_parameter_helper(
  return parameters.at(name).value;
 }

-const rclcpp::ParameterValue &
-NodeParameters::declare_parameter(const std::string & name)
-{
-  rcl_interfaces::msg::ParameterDescriptor descriptor;
-  descriptor.dynamic_typing = true;
-  return this->declare_parameter(name, rclcpp::ParameterValue{}, descriptor, false);
-}
-
 const rclcpp::ParameterValue &
 NodeParameters::declare_parameter(
  const std::string & name,
--- a/Show More
+++ b/Show More