Remove RMW interfaces

Signed-off-by: methylDragon <methylDragon@gmail.com>

.github/workflows/mirror-rolling-to-master.yaml

@@ -0,0 +1,13 @@
+name: Mirror rolling to master
+
+on:
+  push:
+    branches: [ rolling ]
+
+jobs:
+  mirror-to-master:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: zofrex/mirror-branch@v1
+      with:
+        target-branch: master

CODEOWNERS

@@ -0,0 +1,2 @@
+# This file was generated by https://github.com/audrow/update-ros2-repos
+* @ivanpauno @hidmic @wjwwood

README.md

@@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+
 
 ### Examples
 

rclcpp/CHANGELOG.rst

@@ -2,6 +2,107 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+19.3.0 (2023-03-01)
+-------------------
+* Fix memory leak in tracetools::get_symbol() (`#2104 <https://github.com/ros2/rclcpp/issues/2104>`_)
+* Service introspection (`#1985 <https://github.com/ros2/rclcpp/issues/1985>`_)
+* Allow publishing borrowed messages with intra-process enabled (`#2108 <https://github.com/ros2/rclcpp/issues/2108>`_)
+* to fix flaky test about TestTimeSource.callbacks (`#2111 <https://github.com/ros2/rclcpp/issues/2111>`_)
+* Contributors: Brian, Chen Lihui, Christophe Bedard, Miguel Company
+
+19.2.0 (2023-02-24)
+-------------------
+* to create a sublogger while getting child of Logger (`#1717 <https://github.com/ros2/rclcpp/issues/1717>`_)
+* Fix documentation of Context class (`#2107 <https://github.com/ros2/rclcpp/issues/2107>`_)
+* fixes for rmw callbacks in qos_event class (`#2102 <https://github.com/ros2/rclcpp/issues/2102>`_)
+* Contributors: Alberto Soragna, Chen Lihui, Silvio Traversaro
+
+19.1.0 (2023-02-14)
+-------------------
+* Add support for timers on reset callback (`#1979 <https://github.com/ros2/rclcpp/issues/1979>`_)
+* Topic node guard condition in executor (`#2074 <https://github.com/ros2/rclcpp/issues/2074>`_)
+* Fix bug on the disorder of calling shutdown callback (`#2097 <https://github.com/ros2/rclcpp/issues/2097>`_)
+* Contributors: Barry Xu, Chen Lihui, mauropasse
+
+19.0.0 (2023-01-30)
+-------------------
+* Add default constructor to NodeInterfaces (`#2094 <https://github.com/ros2/rclcpp/issues/2094>`_)
+* Fix clock state cached time to be a copy, not a reference. (`#2092 <https://github.com/ros2/rclcpp/issues/2092>`_)
+* Fix -Wmaybe-uninitialized warning (`#2081 <https://github.com/ros2/rclcpp/issues/2081>`_)
+* Fix the keep_last warning when using system defaults. (`#2082 <https://github.com/ros2/rclcpp/issues/2082>`_)
+* Add in a fix for older compilers. (`#2075 <https://github.com/ros2/rclcpp/issues/2075>`_)
+* Contributors: Alexander Hans, Chris Lalancette, Shane Loretz
+
+18.0.0 (2022-12-29)
+-------------------
+* Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)
+* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_)
+* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 <https://github.com/ros2/rclcpp/issues/2066>`_)
+* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_)
+* Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_)
+* Add clock type to node_options (`#1982 <https://github.com/ros2/rclcpp/issues/1982>`_)
+* Fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_)
+* Remove templating on to_rcl_subscription_options (`#2056 <https://github.com/ros2/rclcpp/issues/2056>`_)
+* Fix SharedFuture from async_send_request never becoming valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_)
+* Add in a warning for a KeepLast depth of 0. (`#2048 <https://github.com/ros2/rclcpp/issues/2048>`_)
+* Mark rclcpp::Clock::now() as const (`#2050 <https://github.com/ros2/rclcpp/issues/2050>`_)
+* Fix a case that did not throw ParameterUninitializedException (`#2036 <https://github.com/ros2/rclcpp/issues/2036>`_)
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon
+
+17.1.0 (2022-11-02)
+-------------------
+* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 <https://github.com/ros2/rclcpp/issues/2032>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_)
+* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 <https://github.com/ros2/rclcpp/issues/2030>`_)
+* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_)
+* Set cpplint test timeout to 3 minutes (`#2022 <https://github.com/ros2/rclcpp/issues/2022>`_)
+* Make sure to include-what-you-use in the node_interfaces. (`#2018 <https://github.com/ros2/rclcpp/issues/2018>`_)
+* Do not clear entities callbacks on destruction (`#2002 <https://github.com/ros2/rclcpp/issues/2002>`_)
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_)
+* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks
+
+17.0.0 (2022-09-13)
+-------------------
+* Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 <https://github.com/ros2/rclcpp/issues/1978>`_)
+* support regex match for parameter client (`#1992 <https://github.com/ros2/rclcpp/issues/1992>`_)
+* operator+= and operator-= for Duration (`#1988 <https://github.com/ros2/rclcpp/issues/1988>`_)
+* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 <https://github.com/ros2/rclcpp/issues/1975>`_)" (`#2009 <https://github.com/ros2/rclcpp/issues/2009>`_) (`#2010 <https://github.com/ros2/rclcpp/issues/2010>`_)
+* force compiler warning if callback handles not captured (`#2000 <https://github.com/ros2/rclcpp/issues/2000>`_)
+* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 <https://github.com/ros2/rclcpp/issues/1975>`_)" (`#2009 <https://github.com/ros2/rclcpp/issues/2009>`_)
+* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 <https://github.com/ros2/rclcpp/issues/1975>`_)
+* [docs] add note about callback lifetime for {on, post}_set_parameter_callback (`#1981 <https://github.com/ros2/rclcpp/issues/1981>`_)
+* fix memory leak (`#1994 <https://github.com/ros2/rclcpp/issues/1994>`_)
+* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 <https://github.com/ros2/rclcpp/issues/1947>`_)
+* Make create_service accept rclcpp::QoS (`#1969 <https://github.com/ros2/rclcpp/issues/1969>`_)
+* Make create_client accept rclcpp::QoS (`#1964 <https://github.com/ros2/rclcpp/issues/1964>`_)
+* Fix the documentation for rclcpp::ok to be accurate. (`#1965 <https://github.com/ros2/rclcpp/issues/1965>`_)
+* use regex for wildcard matching (`#1839 <https://github.com/ros2/rclcpp/issues/1839>`_)
+* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 <https://github.com/ros2/rclcpp/issues/1821>`_) (`#1874 <https://github.com/ros2/rclcpp/issues/1874>`_)" (`#1956 <https://github.com/ros2/rclcpp/issues/1956>`_)
+* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 <https://github.com/ros2/rclcpp/issues/1821>`_) (`#1874 <https://github.com/ros2/rclcpp/issues/1874>`_)
+* test adjustment for LoanedMessage. (`#1951 <https://github.com/ros2/rclcpp/issues/1951>`_)
+* fix virtual dispatch issues identified by clang-tidy (`#1816 <https://github.com/ros2/rclcpp/issues/1816>`_)
+* Remove unused on_parameters_set_callback\_ (`#1945 <https://github.com/ros2/rclcpp/issues/1945>`_)
+* Fix subscription.is_serialized() for callbacks with message info (`#1950 <https://github.com/ros2/rclcpp/issues/1950>`_)
+* wait for subscriptions on another thread. (`#1940 <https://github.com/ros2/rclcpp/issues/1940>`_)
+* Fix documentation of `RCLCPP\_[INFO,WARN,...]` (`#1943 <https://github.com/ros2/rclcpp/issues/1943>`_)
+* Always trigger guard condition waitset (`#1923 <https://github.com/ros2/rclcpp/issues/1923>`_)
+* Add statistics for handle_loaned_message (`#1927 <https://github.com/ros2/rclcpp/issues/1927>`_)
+* Drop wrong template specialization (`#1926 <https://github.com/ros2/rclcpp/issues/1926>`_)
+* Contributors: Alberto Soragna, Andrew Symington, Barry Xu, Brian, Chen Lihui, Chris Lalancette, Daniel Reuter, Deepanshu Bansal, Hubert Liberacki, Ivan Santiago Paunovic, Jochen Sprickerhof, Nikolai Morin, Shane Loretz, Tomoya Fujita, Tyler Weaver, William Woodall, schrodinbug
+
+16.2.0 (2022-05-03)
+-------------------
+* Update get_parameter_from_event to follow the function description (`#1922 <https://github.com/ros2/rclcpp/issues/1922>`_)
+* Add 'best available' QoS enum values and methods (`#1920 <https://github.com/ros2/rclcpp/issues/1920>`_)
+* Contributors: Barry Xu, Jacob Perron
+
+16.1.0 (2022-04-29)
+-------------------
+* use reinterpret_cast for function pointer conversion. (`#1919 <https://github.com/ros2/rclcpp/issues/1919>`_)
+* Contributors: Tomoya Fujita
+
 16.0.1 (2022-04-13)
 -------------------
 * remove DEFINE_CONTENT_FILTER cmake option (`#1914 <https://github.com/ros2/rclcpp/issues/1914>`_)

rclcpp/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(tracetools REQUIRED)
 # Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
@@ -48,6 +49,11 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
   src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions/exceptions.cpp
   src/rclcpp/executable_list.cpp
@@ -92,8 +98,9 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/parameter_value.cpp
   src/rclcpp/publisher_base.cpp
   src/rclcpp/qos.cpp
-  src/rclcpp/qos_event.cpp
+  src/rclcpp/event_handler.cpp
   src/rclcpp/qos_overriding_options.cpp
+  src/rclcpp/dynamic_subscription.cpp
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp
@@ -259,4 +266,8 @@ if(TEST cppcheck)
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
 
+if(TEST cpplint)
+  set_tests_properties(cpplint PROPERTIES TIMEOUT 180)
+endif()
+
 ament_generate_version_header(${PROJECT_NAME})

rclcpp/doc/proposed_node_parameter_callbacks.md

@@ -0,0 +1,29 @@
+# Proposed node parameters callback Design
+
+## Introduction:
+
+The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365). 
+
+The main requirement is to set one or more parameters after another parameter is set successfully. 
+
+Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation).
+
+Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789)
+
+With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects. 
+
+There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback.
+
+We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set.
+
+The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`.
+
+It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed.
+To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list. 
+
+![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true)
+
+## Alternatives
+
+* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier).
+* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails.

rclcpp/include/rclcpp/allocator/allocator_common.hpp

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

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -191,10 +191,14 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && arg) {
-        TRACEPOINT(
-          rclcpp_callback_register,
-          static_cast<const void *>(this),
-          tracetools::get_symbol(arg));
+        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(arg);
+          DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
       }, callback_);
 #endif  // TRACETOOLS_DISABLED
   }

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -950,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
@@ -959,10 +965,14 @@ public:
 #ifndef TRACETOOLS_DISABLED
     std::visit(
       [this](auto && callback) {
-        TRACEPOINT(
-          rclcpp_callback_register,
-          static_cast<const void *>(this),
-          tracetools::get_symbol(callback));
+        if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(callback);
+          DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
       }, callback_variant_);
 #endif  // TRACETOOLS_DISABLED
   }

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
@@ -171,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)
 
@@ -213,6 +230,9 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -16,14 +16,15 @@
 #define RCLCPP__CLIENT_HPP_
 
 #include <atomic>
+#include <functional>
 #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>
 #include <tuple>
+#include <unordered_map>
 #include <utility>
 #include <variant>  // NOLINT
 #include <vector>
@@ -31,8 +32,10 @@
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
+#include "rcl/service_introspection.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -130,7 +133,7 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
-  virtual ~ClientBase();
+  virtual ~ClientBase() = default;
 
   /// Take the next response for this client as a type erased pointer.
   /**
@@ -312,7 +315,7 @@ public:
     // 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>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -320,7 +323,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_response_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_response_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_response_callback_));
   }
 
@@ -466,15 +470,13 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph)
+  : ClientBase(node_base, node_graph),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle =
-      get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
       this->get_client_handle().get(),
       this->get_rcl_node_handle(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &client_options);
     if (ret != RCL_RET_OK) {
@@ -769,7 +771,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;
@@ -778,6 +782,33 @@ public:
     return old_size - pending_requests_.size();
   }
 
+  /// Configure client introspection.
+  /**
+   * \param[in] clock clock to use to generate introspection timestamps
+   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_client_configure_service_introspection(
+      client_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection");
+    }
+  }
+
 protected:
   using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
   using CallbackWithRequestTypeValueVariant = std::tuple<
@@ -792,16 +823,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;
   }
 
@@ -830,6 +859,9 @@ protected:
       CallbackInfoVariant>>
   pending_requests_;
   std::mutex pending_requests_mutex_;
+
+private:
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/clock.hpp

@@ -77,7 +77,7 @@ public:
    */
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
 
   /**
    * Sleep until a specified Time, according to clock type.
@@ -137,6 +137,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.
    *

rclcpp/include/rclcpp/context.hpp

@@ -65,8 +65,11 @@ using PreShutdownCallbackHandle = ShutdownCallbackHandle;
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
- * It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
- * and rclcpp::shutdown.
+ * Nodes may be attached to a particular context by passing to the rclcpp::Node
+ * constructor a rclcpp::NodeOptions instance in which the Context is set via
+ * rclcpp::NodeOptions::context.
+ * Nodes will be automatically removed from the context when destructed.
+ * Contexts may be shutdown by calling rclcpp::shutdown.
  */
 class Context : public std::enable_shared_from_this<Context>
 {
@@ -183,6 +186,11 @@ public:
    *
    * This function is thread-safe.
    *
+   * Note that if you override this method, but leave shutdown to be called in
+   * the destruction of this base class, it will not call the overridden
+   * version from your base class.
+   * So you need to ensure you call your class's shutdown() in its destructor.
+   *
    * \param[in] reason the description of why shutdown happened
    * \return true if shutdown was successful, false if context was already shutdown
    * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
@@ -318,7 +326,6 @@ public:
 
   /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
   RCLCPP_PUBLIC
-  virtual
   void
   interrupt_all_sleep_for();
 
@@ -351,7 +358,6 @@ protected:
   // Called by constructor and destructor to clean up by finalizing the
   // shutdown rcl context and preparing for a new init cycle.
   RCLCPP_PUBLIC
-  virtual
   void
   clean_up();
 
@@ -373,10 +379,10 @@ private:
   // attempt to acquire another sub context.
   std::recursive_mutex sub_contexts_mutex_;
 
-  std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
-  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
   mutable std::mutex pre_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).
@@ -395,20 +401,22 @@ private:
 
   using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
 
+  template<ShutdownType shutdown_type>
   RCLCPP_LOCAL
   ShutdownCallbackHandle
   add_shutdown_callback(
-    ShutdownType shutdown_type,
     ShutdownCallback callback);
 
+  template<ShutdownType shutdown_type>
   RCLCPP_LOCAL
   bool
   remove_shutdown_callback(
-    ShutdownType shutdown_type,
     const ShutdownCallbackHandle & callback_handle);
 
+  template<ShutdownType shutdown_type>
+  RCLCPP_LOCAL
   std::vector<rclcpp::Context::ShutdownCallback>
-  get_shutdown_callback(ShutdownType shutdown_type) const;
+  get_shutdown_callback() const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/create_client.hpp

@@ -20,10 +20,40 @@
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/rmw.h"
 
 namespace rclcpp
 {
+/// Create a service client with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_graph NodeGraphInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the client.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] qos Quality of service profile for client.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created client.
+ */
+template<typename ServiceT>
+typename rclcpp::Client<ServiceT>::SharedPtr
+create_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_client<ServiceT>(
+    node_base, node_graph, node_services,
+    service_name,
+    qos.get_rmw_qos_profile(),
+    group);
+}
 
 /// Create a service client with a given type.
 /// \internal

rclcpp/include/rclcpp/create_service.hpp

@@ -26,6 +26,32 @@
 
 namespace rclcpp
 {
+/// Create a service with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the service.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return create_service<ServiceT, CallbackT>(
+    node_base, node_services, service_name,
+    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+}
 
 /// Create a service with a given type.
 /// \internal

rclcpp/include/rclcpp/create_timer.hpp

@@ -23,85 +23,29 @@
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 
 namespace rclcpp
 {
-/// Create a timer with a given clock
-/// \internal
-template<typename CallbackT>
-typename rclcpp::TimerBase::SharedPtr
-create_timer(
-  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
-  rclcpp::Clock::SharedPtr clock,
-  rclcpp::Duration period,
-  CallbackT && callback,
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+namespace detail
 {
-  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
-    clock,
-    period.to_chrono<std::chrono::nanoseconds>(),
-    std::forward<CallbackT>(callback),
-    node_base->get_context());
-
-  node_timers->add_timer(timer, group);
-  return timer;
-}
-
-/// Create a timer with a given clock
-template<typename NodeT, typename CallbackT>
-typename rclcpp::TimerBase::SharedPtr
-create_timer(
-  NodeT node,
-  rclcpp::Clock::SharedPtr clock,
-  rclcpp::Duration period,
-  CallbackT && callback,
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
-{
-  return create_timer(
-    rclcpp::node_interfaces::get_node_base_interface(node),
-    rclcpp::node_interfaces::get_node_timers_interface(node),
-    clock,
-    period,
-    std::forward<CallbackT>(callback),
-    group);
-}
-
-/// Convenience method to create a timer with node resources.
+/// Perform a safe cast to a timer period in nanoseconds
 /**
  *
  * \tparam DurationRepT
  * \tparam DurationT
- * \tparam CallbackT
  * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
- * \param callback callback to execute via the timer period
- * \param group
- * \param node_base
- * \param node_timers
- * \return
- * \throws std::invalid argument if either node_base or node_timers
- * are null, or period is negative or too large
+ * \return period, expressed as chrono::duration::nanoseconds
+ * \throws std::invalid_argument if period is negative or too large
  */
-template<typename DurationRepT, typename DurationT, typename CallbackT>
-typename rclcpp::WallTimer<CallbackT>::SharedPtr
-create_wall_timer(
-  std::chrono::duration<DurationRepT, DurationT> period,
-  CallbackT callback,
-  rclcpp::CallbackGroup::SharedPtr group,
-  node_interfaces::NodeBaseInterface * node_base,
-  node_interfaces::NodeTimersInterface * node_timers)
+template<typename DurationRepT, typename DurationT>
+std::chrono::nanoseconds
+safe_cast_to_period_in_ns(std::chrono::duration<DurationRepT, DurationT> period)
 {
-  if (node_base == nullptr) {
-    throw std::invalid_argument{"input node_base cannot be null"};
-  }
-
-  if (node_timers == nullptr) {
-    throw std::invalid_argument{"input node_timers cannot be null"};
-  }
-
   if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
     throw std::invalid_argument{"timer period cannot be negative"};
   }
@@ -132,12 +76,135 @@ create_wall_timer(
             "Casting timer period to nanoseconds resulted in integer overflow."};
   }
 
+  return period_ns;
+}
+}  // namespace detail
+
+/// Create a timer with a given clock
+/// \internal
+template<typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_timer(
+    clock,
+    period.to_chrono<std::chrono::nanoseconds>(),
+    std::forward<CallbackT>(callback),
+    group,
+    node_base.get(),
+    node_timers.get());
+}
+
+/// Create a timer with a given clock
+template<typename NodeT, typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  NodeT node,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_timer(
+    clock,
+    period.to_chrono<std::chrono::nanoseconds>(),
+    std::forward<CallbackT>(callback),
+    group,
+    rclcpp::node_interfaces::get_node_base_interface(node).get(),
+    rclcpp::node_interfaces::get_node_timers_interface(node).get());
+}
+
+/// Convenience method to create a general timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param clock clock to be used
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \return shared pointer to a generic timer
+ * \throws std::invalid_argument if either clock, node_base or node_timers
+ * are nullptr, or period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+create_timer(
+  rclcpp::Clock::SharedPtr clock,
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (clock == nullptr) {
+    throw std::invalid_argument{"clock cannot be null"};
+  }
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new generic timer.
+  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
+    std::move(clock), period_ns, std::move(callback), node_base->get_context());
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
+/// Convenience method to create a wall timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max()
+ * \param callback callback to execute via the timer period
+ * \param group callback group
+ * \param node_base node base interface
+ * \param node_timers node timer interface
+ * \return shared pointer to a wall timer
+ * \throws std::invalid_argument if either node_base or node_timers
+ * are null, or period is negative or too large
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);
+
+  // Add a new wall timer.
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
     period_ns, std::move(callback), node_base->get_context());
   node_timers->add_timer(timer, group);
   return timer;
 }
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -41,6 +41,7 @@ namespace detail
  * so no exceptions should be thrown at this point, and doing so results in
  * undefined behavior.
  *
+ * \tparam UserDataRealT Declared type of the passed function
  * \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
@@ -50,6 +51,7 @@ namespace detail
  * \returns whatever the callback returns, if anything
  */
 template<
+  typename UserDataRealT,
   typename UserDataT,
   typename ... Args,
   typename ReturnT = void
@@ -57,7 +59,7 @@ template<
 ReturnT
 cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
 {
-  auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
+  auto & actual_callback = *static_cast<const UserDataRealT *>(user_data);
   return actual_callback(args ...);
 }
 

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_

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_

rclcpp/include/rclcpp/duration.hpp

@@ -92,9 +92,13 @@ public:
   Duration
   operator+(const rclcpp::Duration & rhs) const;
 
+  Duration & operator+=(const rclcpp::Duration & rhs);
+
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  Duration & operator-=(const rclcpp::Duration & rhs);
+
   /// Get the maximum representable value.
   /**
    * \return the maximum representable value
@@ -106,6 +110,9 @@ public:
   Duration
   operator*(double scale) const;
 
+  Duration &
+  operator*=(double scale);
+
   /// Get duration in nanosecods
   /**
    * \return the duration in nanoseconds as a rcl_duration_value_t.

rclcpp/include/rclcpp/dynamic_subscription.hpp

@@ -0,0 +1,176 @@
+// 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__DYNAMIC_SUBSCRIPTION_HPP_
+#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
+
+#include <rosidl_dynamic_typesupport/identifier.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// %Subscription for messages whose type descriptions are obtained at runtime.
+/**
+ * Since the type is not known at compile time, this is not a template, and the dynamic library
+ * containing type support information has to be identified and loaded based on the type name.
+ *
+ * NOTE(methylDragon): No considerations for intra-process handling are made.
+ */
+class DynamicSubscription : public rclcpp::SubscriptionBase
+{
+public:
+  // cppcheck-suppress unknownMacro
+  RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription)
+
+  template<typename AllocatorT = std::allocator<void>>
+  DynamicSubscription(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    std::function<void(
+      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
+      std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
+    )> callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+    bool use_take_dynamic_message = true)
+  : SubscriptionBase(
+      node_base,
+      *(type_support->get_rosidl_message_type_support()),
+      topic_name,
+      options.to_rcl_subscription_options(
+        qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
+      use_take_dynamic_message ? SubscriptionType::DYNAMIC_MESSAGE_DIRECT : SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED),  // NOLINT
+    ts_(type_support),
+    callback_(callback),
+    serialization_support_(nullptr),
+    dynamic_message_(nullptr),
+    dynamic_message_type_(nullptr)
+  {
+    if (!type_support) {
+      throw std::runtime_error("DynamicMessageTypeSupport cannot be nullptr!");
+    }
+
+    if (type_support->get_rosidl_message_type_support()->typesupport_identifier !=
+      rosidl_get_dynamic_typesupport_identifier())
+    {
+      throw std::runtime_error(
+              "DynamicSubscription must use dynamic type introspection type support!");
+    }
+
+    serialization_support_ = type_support->get_shared_dynamic_serialization_support();
+    dynamic_message_type_ = type_support->get_shared_dynamic_message_type()->clone_shared();
+    dynamic_message_ = type_support->get_shared_dynamic_message()->clone_shared();
+  }
+
+  // TODO(methylDragon):
+  /// Deferred type description constructor, only usable if the middleware implementation supports
+  /// type discovery
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicSubscription() = default;
+
+  // Same as create_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  std::shared_ptr<void> create_message() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
+
+  /// Cast the message to a rclcpp::SerializedMessage and call the callback.
+  RCLCPP_PUBLIC
+  void handle_message(
+    std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
+
+  /// Handle dispatching rclcpp::SerializedMessage to user callback.
+  RCLCPP_PUBLIC
+  void
+  handle_serialized_message(
+    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
+    const rclcpp::MessageInfo & message_info) override;
+
+  /// This function is currently not implemented.
+  RCLCPP_PUBLIC
+  void handle_loaned_message(
+    void * loaned_message, const rclcpp::MessageInfo & message_info) override;
+
+  // Same as return_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  void return_message(std::shared_ptr<void> & message) override;
+
+  RCLCPP_PUBLIC
+  void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
+
+
+  // DYNAMIC TYPE ==================================================================================
+  // TODO(methylDragon): Reorder later
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  void return_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
+
+  RCLCPP_PUBLIC
+  void handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) override;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicSubscription)
+
+  rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_;
+  std::function<void(
+      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
+      std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
+    )> callback_;
+
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_;
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_;
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr dynamic_message_type_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp

@@ -0,0 +1,327 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#endif
+
+
+#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicMessage::get_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id) \
+  { \
+    ValueT out; \
+    rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \
+      rosidl_dynamic_data_.get(), id, &out); \
+    return out; \
+  }
+
+#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicMessage::get_value<ValueT>(const std::string & name) \
+  { \
+    return get_value<ValueT>(get_member_id(name)); \
+  }
+
+#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessage::set_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \
+      rosidl_dynamic_data_.get(), id, value); \
+  }
+
+#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessage::set_value<ValueT>(const std::string & name, ValueT value) \
+  { \
+    set_value<ValueT>(get_member_id(name), value); \
+  }
+
+#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \
+  template<> \
+  rosidl_dynamic_typesupport_member_id_t \
+  DynamicMessage::insert_value<ValueT>(ValueT value) \
+  { \
+    rosidl_dynamic_typesupport_member_id_t out; \
+    rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \
+      rosidl_dynamic_data_.get(), value, &out); \
+    return out; \
+  }
+
+#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT)
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/**
+ * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+ * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+ *
+ * Explicitly:
+ * - Basic types:        bool, byte, char
+ * - Float types:        float, double
+ * - Int types:          int8_t, int16_t, int32_t, int64_t
+ * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+ * - String types:       std::string, std::u16string
+ */
+
+DYNAMIC_MESSAGE_DEFINITIONS(bool, bool);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte);
+DYNAMIC_MESSAGE_DEFINITIONS(char, char);
+DYNAMIC_MESSAGE_DEFINITIONS(float, float32);
+DYNAMIC_MESSAGE_DEFINITIONS(double, float64);
+DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8);
+DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16);
+DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32);
+DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64);
+DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string);
+
+// Byte and String getters have a different implementation and are defined below
+
+
+// BYTE ============================================================================================
+template<>
+std::byte
+DynamicMessage::get_value<std::byte>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  unsigned char out;
+  rosidl_dynamic_typesupport_dynamic_data_get_byte_value(get_rosidl_dynamic_data(), id, &out);
+  return static_cast<std::byte>(out);
+}
+
+
+template<>
+std::byte
+DynamicMessage::get_value<std::byte>(const std::string & name)
+{
+  return get_value<std::byte>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::byte>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::byte value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_byte_value(
+    rosidl_dynamic_data_.get(), id, static_cast<unsigned char>(value));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::byte>(const std::string & name, const std::byte value)
+{
+  set_value<std::byte>(get_member_id(name), value);
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::byte>(const std::byte value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_byte_value(
+    rosidl_dynamic_data_.get(), static_cast<unsigned char>(value), &out);
+  return out;
+}
+
+
+// STRINGS =========================================================================================
+template<>
+std::string
+DynamicMessage::get_value<std::string>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_string_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+template<>
+std::u16string
+DynamicMessage::get_value<std::u16string>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_wstring_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+template<>
+std::string
+DynamicMessage::get_value<std::string>(const std::string & name)
+{
+  return get_value<std::string>(get_member_id(name));
+}
+
+
+template<>
+std::u16string
+DynamicMessage::get_value<std::u16string>(const std::string & name)
+{
+  return get_value<std::u16string>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::string>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_string_value(
+    rosidl_dynamic_data_.get(), id, value.c_str(), value.size());
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::u16string>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_wstring_value(
+    rosidl_dynamic_data_.get(), id, value.c_str(), value.size());
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::string>(const std::string & name, const std::string value)
+{
+  set_value<std::string>(get_member_id(name), value);
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::u16string>(const std::string & name, const std::u16string value)
+{
+  set_value<std::u16string>(get_member_id(name), value);
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::string>(const std::string value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_string_value(
+    rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out);
+  return out;
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::u16string>(const std::u16string value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value(
+    rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out);
+  return out;
+}
+
+
+// THROW FOR UNSUPPORTED TYPES =====================================================================
+template<typename ValueT>
+ValueT
+DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+ValueT
+DynamicMessage::get_value(const std::string & name)
+{
+  throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+void
+DynamicMessage::set_value(
+  rosidl_dynamic_typesupport_member_id_t id, ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+void
+DynamicMessage::set_value(const std::string & name, ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value(ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN
+#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN
+#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_MESSAGE_INSERT_VALUE
+#undef DYNAMIC_MESSAGE_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp

@@ -0,0 +1,189 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
+#endif
+
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \
+      rosidl_dynamic_type_builder_.get(), \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_array_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \
+    size_t array_length, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \
+      rosidl_dynamic_type_builder_.get(), \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
+      array_length); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_unbounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \
+    _unbounded_sequence_member( \
+      rosidl_dynamic_type_builder_.get(), \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_bounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    size_t sequence_bound, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \
+      rosidl_dynamic_type_builder_.get(), \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
+      sequence_bound); \
+  }
+
+#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/**
+ * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+ * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+ *
+ * Explicitly:
+ * - Basic types:        bool, byte, char
+ * - Float types:        float, double
+ * - Int types:          int8_t, int16_t, int32_t, int64_t
+ * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+ * - String types:       std::string, std::u16string
+ */
+
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring);
+
+
+// THROW FOR UNSUPPORTED TYPES =====================================================================
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_array_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  size_t array_length, const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_array_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_unbounded_sequence_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  size_t sequence_bound,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_bounded_sequence_member is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN
+#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -0,0 +1,430 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+
+#include <rcl/types.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+class DynamicMessageType;
+class DynamicMessageTypeBuilder;
+
+/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t *
+/**
+ * This class:
+ * - Manages the lifetime of the raw pointer.
+ * - Exposes getter methods to get the raw pointer and shared pointers
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed
+ *   DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport.
+ * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer
+ *   must point to the same location in memory as the stored raw pointer!
+ *
+ * Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t,
+ *       even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to
+ *       rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to
+ *       rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields.
+ */
+class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
+
+  // CONSTRUCTION ==================================================================================
+  // Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the
+  // lifetime of the serialization support (if the constructor cannot otherwise get it from args).
+  //
+  // In cases where a dynamic data pointer is passed, the serialization support composed by
+  // the data should be the exact same object managed by the DynamicSerializationSupport,
+  // otherwise the lifetime management will not work properly.
+
+  /// Construct a new DynamicMessage with the provided dynamic type builder
+  RCLCPP_PUBLIC
+  explicit DynamicMessage(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
+
+  /// Construct a new DynamicMessage with the provided dynamic type
+  RCLCPP_PUBLIC
+  explicit DynamicMessage(std::shared_ptr<DynamicMessageType> dynamic_type);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data);
+
+  /// Loaning constructor
+  /// Must only be called with raw ptr obtained from loaning!
+  // NOTE(methylDragon): I'd put this in protected, but I need this exposed to
+  //                     enable_shared_from_this...
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    DynamicMessage::SharedPtr parent_data,
+    rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data);
+
+  // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using
+  //                     construction from dynamic type/builder, which is more efficient
+
+  /// Copy constructor
+  RCLCPP_PUBLIC
+  DynamicMessage(const DynamicMessage & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicMessage(DynamicMessage && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicMessage & operator=(const DynamicMessage & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicMessage & operator=(DynamicMessage && other) noexcept;
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessage();
+
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_name() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_dynamic_data_t *
+  get_rosidl_dynamic_data();
+
+  RCLCPP_PUBLIC
+  const rosidl_dynamic_typesupport_dynamic_data_t *
+  get_rosidl_dynamic_data() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
+  get_shared_rosidl_dynamic_data();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_data_t>
+  get_shared_rosidl_dynamic_data() const;
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_item_count() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_member_id(size_t index) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_member_id(const std::string & name) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_array_index(size_t index) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_array_index(const std::string & name) const;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  DynamicMessage
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  init_from_type(DynamicMessageType & type) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  init_from_type_shared(DynamicMessageType & type) const;
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicMessage & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  loan_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  loan_value(const std::string & name);
+
+  RCLCPP_PUBLIC
+  void
+  clear_all_values();
+
+  RCLCPP_PUBLIC
+  void
+  clear_nonkey_values();
+
+  RCLCPP_PUBLIC
+  void
+  clear_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  void
+  clear_value(const std::string & name);
+
+  RCLCPP_PUBLIC
+  void
+  clear_sequence();
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_sequence_data();
+
+  RCLCPP_PUBLIC
+  void
+  remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index);
+
+  RCLCPP_PUBLIC
+  bool
+  serialize(rcl_serialized_message_t * buffer);
+
+  RCLCPP_PUBLIC
+  bool
+  deserialize(rcl_serialized_message_t * buffer);
+
+
+  // MEMBER ACCESS TEMPLATES =======================================================================
+  /**
+   * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+   * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+   *
+   * Explicitly:
+   * - Basic types:        bool, byte, char
+   * - Float types:        float, double
+   * - Int types:          int8_t, int16_t, int32_t, int64_t
+   * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+   * - String types:       std::string, std::u16string
+   */
+
+  template<typename ValueT>
+  ValueT
+  get_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  template<typename ValueT>
+  ValueT
+  get_value(const std::string & name);
+
+  template<typename ValueT>
+  void
+  set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value);
+
+  template<typename ValueT>
+  void
+  set_value(const std::string & name, ValueT value);
+
+  template<typename ValueT>
+  rosidl_dynamic_typesupport_member_id_t
+  insert_value(ValueT value);
+
+
+  // FIXED STRING MEMBER ACCESS ====================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length);
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_fixed_string_value(const std::string & name, size_t string_length);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_fixed_wstring_value(const std::string & name, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_string_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_string_value(const std::string & name, const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_wstring_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_wstring_value(
+    const std::string & name, const std::u16string value, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_fixed_string_value(const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_fixed_wstring_value(const std::u16string value, size_t wstring_length);
+
+
+  // BOUNDED STRING MEMBER ACCESS ==================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_bounded_string_value(const std::string & name, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_bounded_wstring_value(const std::string & name, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_string_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_wstring_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_wstring_value(
+    const std::string & name, const std::u16string value, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_bounded_string_value(const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound);
+
+
+  // NESTED MEMBER ACCESS ==========================================================================
+  RCLCPP_PUBLIC
+  DynamicMessage
+  get_complex_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  get_complex_value(const std::string & name);
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_complex_value_shared(const std::string & name);
+
+  RCLCPP_PUBLIC
+  void
+  set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  void
+  set_complex_value(const std::string & name, DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_complex_value_copy(const DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_complex_value(DynamicMessage & value);
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // DynamicSerializationSupport
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data_;
+
+  bool is_loaned_;
+
+  // Used for returning the loaned value, and lifetime management
+  DynamicMessage::SharedPtr parent_data_;
+
+private:
+  RCLCPP_PUBLIC
+  DynamicMessage();
+
+  RCLCPP_PUBLIC
+  bool
+  match_serialization_support_(
+    const DynamicSerializationSupport & serialization_support,
+    const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data);
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp

@@ -0,0 +1,207 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+class DynamicMessage;
+class DynamicMessageTypeBuilder;
+
+/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t *`
+/**
+ * This class:
+ * - Manages the lifetime of the raw pointer.
+ * - Exposes getter methods to get the raw pointer and shared pointers
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the
+ *   passed `DynamicSerializationSupport`.
+ *   So it cannot outlive the `DynamicSerializationSupport`.
+ * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t`
+ *   pointer must point to the same location in memory as the stored raw pointer!
+ *
+ * This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`,
+ * which can be constructed via `DynamicMessageTypeBuilder`, which maps to
+ * `rosidl_dynamic_typesupport_dynamic_type_builder_t`.
+ *
+ *  The usual method of obtaining a `DynamicMessageType` is through construction of
+ *  `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then
+ *  taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain
+ *  `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level
+ *  rosidl methods.
+ */
+class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
+
+  // CONSTRUCTION ==================================================================================
+  // Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the
+  // lifetime of the serialization support (if the constructor cannot otherwise get it from args).
+  //
+  // In cases where a dynamic type pointer is passed, the serialization support composed by
+  // the type should be the exact same object managed by the `DynamicSerializationSupport`,
+  // otherwise the lifetime management will not work properly.
+
+  /// Construct a new `DynamicMessageType` with the provided dynamic type builder
+  RCLCPP_PUBLIC
+  explicit DynamicMessageType(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type);
+
+  /// From description
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription & description);
+
+  /// Copy constructor
+  RCLCPP_PUBLIC
+  DynamicMessageType(const DynamicMessageType & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicMessageType(DynamicMessageType && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicMessageType & operator=(const DynamicMessageType & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicMessageType & operator=(DynamicMessageType && other) noexcept;
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageType();
+
+  /// Swaps the serialization support if `serialization_support` is populated
+  RCLCPP_PUBLIC
+  void
+  init_from_description(
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    DynamicSerializationSupport::SharedPtr serialization_support = nullptr);
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_name() const;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_member_count() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_dynamic_type_t *
+  get_rosidl_dynamic_type();
+
+  RCLCPP_PUBLIC
+  const rosidl_dynamic_typesupport_dynamic_type_t *
+  get_rosidl_dynamic_type() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
+  get_shared_rosidl_dynamic_type();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_t>
+  get_shared_rosidl_dynamic_type() const;
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  DynamicMessageType
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicMessageType & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  build_dynamic_message();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<DynamicMessage>
+  build_dynamic_message_shared();
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // `DynamicSerializationSupport`
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type_;
+
+private:
+  RCLCPP_PUBLIC
+  DynamicMessageType();
+
+  RCLCPP_PUBLIC
+  bool
+  match_serialization_support_(
+    const DynamicSerializationSupport & serialization_support,
+    const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type);
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp

@@ -0,0 +1,414 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+class DynamicMessage;
+class DynamicMessageType;
+
+/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *`
+/**
+ * This class:
+ * - Manages the lifetime of the raw pointer.
+ * - Exposes getter methods to get the raw pointer and shared pointers
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed
+ *   `DynamicSerializationSupport`.
+ *   So it cannot outlive the `DynamicSerializationSupport`.
+ * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t`
+ *   pointer must point to the same location in memory as the stored raw pointer!
+ *
+ * This class is meant to map to rosidl_dynamic_typesupport_dynamic_type_builder_t, facilitating the
+ * construction of dynamic types bottom-up in the C++ layer.
+ *
+ * The usual method of obtaining a `DynamicMessageType` is through construction of
+ * `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then
+ * taking ownership of its contents.
+ * But `DynamicMessageTypeBuilder` can also be used to obtain `DynamicMessageType` by constructing
+ * it bottom-up instead, since it exposes the lower_level rosidl methods.
+ */
+class DynamicMessageTypeBuilder : public std::enable_shared_from_this<DynamicMessageTypeBuilder>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder)
+
+  // CONSTRUCTION ==================================================================================
+  // All constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the
+  // lifetime of the serialization support.
+  //
+  // In cases where a dynamic type builder pointer is passed, the serialization support composed by
+  // the builder should be the exact same object managed by the `DynamicSerializationSupport`,
+  // otherwise the lifetime management will not work properly.
+
+  /// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const std::string & name);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_type_builder_t * dynamic_type_builder);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> dynamic_type_builder);
+
+  /// Copy constructor
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder & operator=(const DynamicMessageTypeBuilder & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept;
+
+  /// From description
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription & description);
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageTypeBuilder();
+
+  /// Swaps the serialization support if serialization_support is populated
+  RCLCPP_PUBLIC
+  void
+  init_from_description(
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    DynamicSerializationSupport::SharedPtr serialization_support = nullptr);
+
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_name() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_dynamic_type_builder_t *
+  get_rosidl_dynamic_type_builder();
+
+  RCLCPP_PUBLIC
+  const rosidl_dynamic_typesupport_dynamic_type_builder_t *
+  get_rosidl_dynamic_type_builder() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>
+  get_shared_rosidl_dynamic_type_builder();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_builder_t>
+  get_shared_rosidl_dynamic_type_builder() const;
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  void
+  set_name(const std::string & name);
+
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  build_dynamic_message();
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  build_dynamic_message_shared();
+
+  RCLCPP_PUBLIC
+  DynamicMessageType
+  build_dynamic_message_type();
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::SharedPtr
+  build_dynamic_message_type_shared();
+
+
+  // ADD MEMBERS TEMPLATES =========================================================================
+  /**
+   * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+   * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+   *
+   * Explicitly:
+   * - Basic types:        bool, byte, char
+   * - Float types:        float, double
+   * - Int types:          int8_t, int16_t, int32_t, int64_t
+   * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+   * - String types:       std::string, std::u16string
+   */
+
+  template<typename MemberT>
+  void
+  add_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    const std::string & default_value = "");
+
+  template<typename MemberT>
+  void
+  add_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length,
+    const std::string & default_value = "");
+
+  template<typename MemberT>
+  void
+  add_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    const std::string & default_value = "");
+
+  template<typename MemberT>
+  void
+  add_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound,
+    const std::string & default_value = "");
+
+
+  // ADD FIXED STRING MEMBERS ======================================================================
+  RCLCPP_PUBLIC
+  void
+  add_fixed_string_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_wstring_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_string_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t string_length, size_t array_length, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_wstring_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t wstring_length, size_t array_length, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_string_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_wstring_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_string_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t string_length, size_t sequence_bound, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_fixed_wstring_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t wstring_length, size_t sequence_bound, const std::string & default_value = "");
+
+
+  // ADD BOUNDED STRING MEMBERS ====================================================================
+  RCLCPP_PUBLIC
+  void
+  add_bounded_string_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_string_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t string_bound, size_t array_length, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t wstring_bound, size_t array_length, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_string_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_string_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t string_bound, size_t sequence_bound, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    size_t wstring_bound, size_t sequence_bound, const std::string & default_value = "");
+
+
+  // ADD NESTED MEMBERS ============================================================================
+  RCLCPP_PUBLIC
+  void
+  add_complex_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageType & nested_type, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageType & nested_type, size_t array_length, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageType & nested_type, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageType & nested_type, size_t sequence_bound,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_array_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageTypeBuilder & nested_type_builder, size_t array_length,
+    const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_unbounded_sequence_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = "");
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_bounded_sequence_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound,
+    const std::string & default_value = "");
+
+protected:
+  // NOTE(methylDragon):
+  // This is just here to extend the lifetime of the serialization support
+  // It isn't actually used by the builder since the builder should compose its own support
+  //
+  // ... Though ideally it should be the exact same support as the one stored in the
+  // `DynamicSerializationSupport`
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> rosidl_dynamic_type_builder_;
+
+private:
+  RCLCPP_PUBLIC
+  DynamicMessageTypeBuilder();
+
+  RCLCPP_PUBLIC
+  void
+  init_from_serialization_support_(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const std::string & name);
+
+  RCLCPP_PUBLIC
+  bool
+  match_serialization_support_(
+    const DynamicSerializationSupport & serialization_support,
+    const rosidl_dynamic_typesupport_dynamic_type_builder_t & dynamic_type_builder);
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -0,0 +1,205 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
+
+#include <rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h>
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <rosidl_runtime_c/type_description/type_description__struct.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for `rosidl_message_type_support_t *` containing managed
+/// instances of the typesupport handle impl.
+/**
+ *
+ * NOTE: This class is the recommended way to obtain the dynamic message type
+ *       support struct, instead of `rcl_dynamic_message_type_support_handle_create()`,
+ *       because this class will manage the lifetimes for you.
+ *
+ *       Do NOT call rcl_dynamic_message_type_support_handle_destroy!!
+ *
+ * This class:
+ * - Manages the lifetime of the raw pointer.
+ * - Exposes getter methods to get the raw pointer and shared pointers
+ * - Stores shared pointers to wrapper classes that expose the underlying
+ *   serialization support API
+ *
+ * Ownership:
+ * - This class, similarly to the `rosidl_dynamic_typesupport_serialization_support_t`, must outlive
+ *   all downstream usages of the serialization support.
+ */
+class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
+
+  // CONSTRUCTION ==================================================================================
+  /// From description
+  /// Does NOT take ownership of the description (copies instead.)
+  /// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    const std::string & serialization_library_name = "");
+
+  /// From description, for provided serialization support
+  /// Does NOT take ownership of the description (copies instead.)
+  /// Constructs type support top-down (calling
+  /// `rosidl_dynamic_message_type_support_handle_create()`)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription & description);
+
+  /// Assume ownership of managed types
+  /// Does NOT take ownership of the description (copies instead.)
+  ///
+  /// The serialization support used to construct all managed SharedPtrs must match.
+  /// The structure of the provided `description` must match the `dynamic_message_type`
+  /// The structure of the provided `dynamic_message_type` must match the `dynamic_message
+  ///
+  /// In this case, the user would have constructed the type support bototm-up (by creating the
+  /// respective dynamic members.)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    DynamicMessageType::SharedPtr dynamic_message_type,
+    DynamicMessage::SharedPtr dynamic_message,
+    const rosidl_runtime_c__type_description__TypeDescription & description);
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicMessageTypeSupport();
+
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  rosidl_message_type_support_t *
+  get_rosidl_message_type_support();
+
+  RCLCPP_PUBLIC
+  const rosidl_message_type_support_t *
+  get_rosidl_message_type_support() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_message_type_support_t>
+  get_shared_rosidl_message_type_support();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_message_type_support_t>
+  get_shared_rosidl_message_type_support() const;
+
+  RCLCPP_PUBLIC
+  rosidl_runtime_c__type_description__TypeDescription *
+  get_rosidl_runtime_c_type_description();
+
+  RCLCPP_PUBLIC
+  const rosidl_runtime_c__type_description__TypeDescription *
+  get_rosidl_runtime_c_type_description() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription>
+  get_shared_rosidl_runtime_c_type_description();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
+  get_shared_rosidl_runtime_c_type_description() const;
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type();
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::ConstSharedPtr
+  get_shared_dynamic_message_type() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_shared_dynamic_message();
+
+  RCLCPP_PUBLIC
+  DynamicMessage::ConstSharedPtr
+  get_shared_dynamic_message() const;
+
+protected:
+  RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
+
+  DynamicSerializationSupport::SharedPtr serialization_support_;
+  DynamicMessageType::SharedPtr dynamic_message_type_;
+  DynamicMessage::SharedPtr dynamic_message_;
+  std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription> description_;
+
+  std::shared_ptr<rosidl_message_type_support_t> rosidl_message_type_support_;
+
+private:
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport();
+
+  RCLCPP_PUBLIC
+  void
+  manage_description_(rosidl_runtime_c__type_description__TypeDescription * description);
+
+  RCLCPP_PUBLIC
+  void
+  init_dynamic_message_type_(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription * description);
+
+  RCLCPP_PUBLIC
+  void
+  init_dynamic_message_(DynamicMessageType::SharedPtr dynamic_type);
+
+  // By aggregation
+  RCLCPP_PUBLIC
+  void
+  init_rosidl_message_type_support_(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    DynamicMessageType::SharedPtr dynamic_message_type,
+    DynamicMessage::SharedPtr dynamic_message,
+    rosidl_runtime_c__type_description__TypeDescription * description);
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp

@@ -0,0 +1,109 @@
+// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t *
+/**
+ * This class:
+ * - Manages the lifetime of the raw pointer.
+ * - Exposes getter methods to get the raw pointer and shared pointers
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive
+ *   all downstream usages of the serialization support.
+ */
+class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
+
+  // CONSTRUCTION ==================================================================================
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport();
+
+  /// Get the rmw middleware implementation specific serialization support (configured by name)
+  RCLCPP_PUBLIC
+  explicit DynamicSerializationSupport(const std::string & serialization_library_name);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  explicit DynamicSerializationSupport(
+    rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport(
+    std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> serialization_support);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport(DynamicSerializationSupport && other) noexcept;
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport & operator=(DynamicSerializationSupport && other) noexcept;
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicSerializationSupport();
+
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_serialization_support_t *
+  get_rosidl_serialization_support();
+
+  RCLCPP_PUBLIC
+  const rosidl_dynamic_typesupport_serialization_support_t *
+  get_rosidl_serialization_support() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
+  get_shared_rosidl_serialization_support();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>
+  get_shared_rosidl_serialization_support() const;
+
+protected:
+  RCLCPP_DISABLE_COPY(DynamicSerializationSupport)
+
+  std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> rosidl_serialization_support_;
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_

rclcpp/include/rclcpp/event_handler.hpp

@@ -0,0 +1,311 @@
+// Copyright 2019 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__EVENT_HANDLER_HPP_
+#define RCLCPP__EVENT_HANDLER_HPP_
+
+#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 "rmw/events_statuses/incompatible_type.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
+{
+
+using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
+using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
+using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
+using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
+using QOSMessageLostInfo = rmw_message_lost_status_t;
+using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
+using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
+
+using IncompatibleTypeInfo = rmw_incompatible_type_status_t;
+using MatchedInfo = rmw_matched_status_t;
+
+using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
+using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
+using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
+using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
+using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
+using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
+using QOSRequestedIncompatibleQoSCallbackType =
+  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
+
+using IncompatibleTypeCallbackType = std::function<void (IncompatibleTypeInfo &)>;
+using PublisherMatchedCallbackType = std::function<void (MatchedInfo &)>;
+using SubscriptionMatchedCallbackType = std::function<void (MatchedInfo &)>;
+
+/// Contains callbacks for various types of events a Publisher can receive from the middleware.
+struct PublisherEventCallbacks
+{
+  QOSDeadlineOfferedCallbackType deadline_callback;
+  QOSLivelinessLostCallbackType liveliness_callback;
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  PublisherMatchedCallbackType matched_callback;
+};
+
+/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
+struct SubscriptionEventCallbacks
+{
+  QOSDeadlineRequestedCallbackType deadline_callback;
+  QOSLivelinessChangedCallbackType liveliness_callback;
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
+  QOSMessageLostCallbackType message_lost_callback;
+  IncompatibleTypeCallbackType incompatible_type_callback;
+  SubscriptionMatchedCallbackType matched_callback;
+};
+
+class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+
+  RCLCPP_PUBLIC
+  UnsupportedEventTypeException(
+    const exceptions::RCLErrorBase & base_exc,
+    const std::string & prefix);
+};
+
+class EventHandlerBase : public Waitable
+{
+public:
+  enum class EntityType : std::size_t
+  {
+    Event,
+  };
+
+  RCLCPP_PUBLIC
+  virtual ~EventHandlerBase();
+
+  /// Get the number of ready events
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_events() override;
+
+  /// Add the Waitable to a wait set.
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Check if the Waitable is ready.
+  RCLCPP_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::EventHandlerBase@" << 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::EventHandlerBase@" << 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<decltype(new_callback), 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<
+        decltype(on_new_event_callback_), 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);
+
+  std::recursive_mutex callback_mutex_;
+  std::function<void(size_t)> on_new_event_callback_{nullptr};
+
+  rcl_event_t event_handle_;
+  size_t wait_set_event_index_;
+};
+
+using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase;
+
+template<typename EventCallbackT, typename ParentHandleT>
+class EventHandler : public EventHandlerBase
+{
+public:
+  template<typename InitFuncT, typename EventTypeEnum>
+  EventHandler(
+    const EventCallbackT & callback,
+    InitFuncT init_func,
+    ParentHandleT parent_handle,
+    EventTypeEnum event_type)
+  : parent_handle_(parent_handle), event_callback_(callback)
+  {
+    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) {
+      if (ret == RCL_RET_UNSUPPORTED) {
+        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
+        rcl_reset_error();
+        throw exc;
+      } else {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
+      }
+    }
+  }
+
+  /// Take data so that the callback cannot be scheduled again
+  std::shared_ptr<void>
+  take_data() override
+  {
+    EventCallbackInfoT callback_info;
+    rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
+    if (ret != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't take event info: %s", rcl_get_error_string().str);
+      return nullptr;
+    }
+    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
+  {
+    if (!data) {
+      throw std::runtime_error("'data' is empty");
+    }
+    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
+    event_callback_(*callback_ptr);
+    callback_ptr.reset();
+  }
+
+private:
+  using EventCallbackInfoT = typename std::remove_reference<typename
+      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
+
+  ParentHandleT parent_handle_;
+  EventCallbackT event_callback_;
+};
+
+template<typename EventCallbackT, typename ParentHandleT>
+using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler<EventCallbackT,
+    ParentHandleT>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -315,6 +315,16 @@ public:
   virtual void
   spin_all(std::chrono::nanoseconds max_duration);
 
+
+  /// Collect work once and execute the next available work, optionally within a duration.
+  /**
+   * This function can be overridden.  The default implementation is suitable for
+   * a single-thread model of execution.
+   * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
+   * to block (which may have unintended consequences).
+   * \param[in] timeout The maximum amount of time to spend waiting for work.
+   *   `-1` is potentially block forever waiting for work.
+   */
   RCLCPP_PUBLIC
   virtual void
   spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@@ -413,12 +423,29 @@ public:
   is_spinning();
 
 protected:
+  /// Add a node to executor, execute the next available unit of work, and remove the node.
+  /**
+   * Implementation of spin_node_once using std::chrono::nanoseconds
+   * \param[in] node Shared pointer to the node to add.
+   * \param[in] timeout How long to wait for work to become available. Negative values cause
+   *   spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
+   *   function to be non-blocking.
+   */
   RCLCPP_PUBLIC
   void
   spin_node_once_nanoseconds(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
     std::chrono::nanoseconds timeout);
 
+  /// Collect work and execute available work, optionally within a duration.
+  /**
+   * Implementation of spin_some and spin_all.
+   * The exhaustive flag controls if the function will re-collect available work within the duration.
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * \param[in] exhaustive when set to true, continue to collect work and execute (spin_all)
+   *                       when set to false, return when all collected work is executed (spin_some)
+   */
   RCLCPP_PUBLIC
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
@@ -433,30 +460,60 @@ protected:
   void
   execute_any_executable(AnyExecutable & any_exec);
 
+  /// Run subscription executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the subscription.
+   * \param[in] subscription Subscription to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  /// Run timer executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the timer callback.
+   * \param[in] timer Timer to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  /// Run service server executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service server callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_service(rclcpp::ServiceBase::SharedPtr service);
 
+  /// Run service client executable.
+  /**
+   * Do necessary setup and tear-down as well as executing the service client callback.
+   * \param[in] service Service to execute
+   */
   RCLCPP_PUBLIC
   static void
   execute_client(rclcpp::ClientBase::SharedPtr client);
 
+  /// Block until more work becomes avilable or timeout is reached.
   /**
+   * Builds a set of waitable entities, which are passed to the middleware.
+   * After building wait set, waits on middleware to notify.
+   * \param[in] timeout duration to wait for new work to become available.
    * \throws std::runtime_error if the wait set can be cleared
    */
   RCLCPP_PUBLIC
   void
   wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
+  /// Find node associated with a callback group
+  /**
+   * \param[in] weak_groups_to_nodes map of callback groups to nodes
+   * \param[in] group callback group to find assocatiated node
+   * \return Pointer to associated node if found, else nullptr
+   */
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
@@ -475,6 +532,11 @@ protected:
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
 
+  /// Find the callback group associated with a timer
+  /**
+   * \param[in] timer Timer to find associated callback group
+   * \return Pointer to callback group node if found, else nullptr
+   */
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -502,16 +564,54 @@ protected:
     WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
     bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
 
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * \param[out] any_executable populated union structure of ready executable
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
 
+  /// Check for executable in ready state and populate union structure.
+  /**
+   * This is the implementation of get_next_ready_executable that takes into
+   * account the current state of callback groups' association with nodes and
+   * executors.
+   *
+   * This checks in a particular order for available work:
+   *   * Timers
+   *   * Subscriptions
+   *   * Services
+   *   * Clients
+   *   * Waitable
+   *
+   * If the next executable is not associated with this executor/node pair,
+   * then this method will return false.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] weak_groups_to_nodes mapping of callback groups to nodes
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable_from_map(
     AnyExecutable & any_executable,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
+  /// Wait for executable in ready state and populate union structure.
+  /**
+   * If an executable is ready, it will return immediately, otherwise
+   * block based on the timeout for work to become ready.
+   *
+   * \param[out] any_executable populated union structure of ready executable
+   * \param[in] timeout duration of time to wait for work, a negative value
+   *   (the defualt behavior), will make this function block indefinitely
+   * \return true if an executable was ready and any_executable was populated,
+   *   otherwise false
+   */
   RCLCPP_PUBLIC
   bool
   get_next_executable(
@@ -565,10 +665,19 @@ protected:
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
 
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
+      const rclcpp::GuardCondition *,
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
+
   /// maps nodes to guard conditions
   WeakNodesToGuardConditionsMap
   weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap
   weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -47,7 +47,7 @@ public:
    *
    * \param options common options for all executors
    * \param number_of_threads number of threads to have in the thread pool,
-   *   the default 0 will use the number of cpu cores found instead
+   *   the default 0 will use the number of cpu cores found (minimum of 2)
    * \param yield_before_execute if true std::this_thread::yield() is called
    * \param timeout maximum time to wait
    */

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

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

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -454,6 +454,8 @@ private:
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
           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
           Deleter deleter = message.get_deleter();
@@ -493,6 +495,8 @@ private:
           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();

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -109,8 +109,14 @@ public:
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
     } else {
       unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
     }
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
@@ -138,7 +144,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
     }
 
     rmw_message_info_t msg_info;

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -52,7 +52,7 @@ public:
   {}
 
   RCLCPP_PUBLIC
-  virtual ~SubscriptionIntraProcessBase();
+  virtual ~SubscriptionIntraProcessBase() = default;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/generic_publisher.hpp

@@ -78,38 +78,12 @@ public:
       node_base,
       topic_name,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
-      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos)),
+      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in publisher.hpp.
-    // TODO(nnmm): Deduplicate by moving this into PublisherBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericPublisher() = default;

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -81,45 +81,13 @@ public:
       node_base,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
       topic_name,
-      options.template to_rcl_subscription_options<rclcpp::SerializedMessage>(qos),
-      true),
+      options.to_rcl_subscription_options(qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
+      SubscriptionType::SERIALIZED_MESSAGE),
     callback_(callback),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in subscription.hpp.
-    // TODO(nnmm): Deduplicate by moving this into SubscriptionBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericSubscription() = default;
@@ -155,6 +123,32 @@ public:
   RCLCPP_PUBLIC
   void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
 
+
+  // DYNAMIC TYPE ==================================================================================
+  // TODO(methylDragon): Reorder later
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
+  override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  void return_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
+
+  RCLCPP_PUBLIC
+  void handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) override;
+
 private:
   RCLCPP_DISABLE_COPY(GenericSubscription)
 

rclcpp/include/rclcpp/graph_listener.hpp

@@ -128,6 +128,11 @@ public:
    * If start_if_not_started() was never called, this function still succeeds,
    * but start_if_not_started() still cannot be called after this function.
    *
+   * Note that if you override this method, but leave shutdown to be called in
+   * the destruction of this base class, it will not call the overridden
+   * version from your base class.
+   * So you need to ensure you call your class's shutdown() in its destructor.
+   *
    * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini()
    * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini()
    * \throws std::system_error anything std::mutex::lock() throws

rclcpp/include/rclcpp/guard_condition.hpp

@@ -72,7 +72,7 @@ public:
   const rcl_guard_condition_t &
   get_rcl_guard_condition() const;
 
-  /// Notify the wait set waiting on this condition, if any, that the condition had been met.
+  /// Signal that the condition has been met, notifying both the wait set and listeners, if any.
   /**
    * This function is thread-safe, and may be called concurrently with waiting
    * on this guard condition in a wait set.
@@ -107,6 +107,22 @@ public:
   void
   add_to_wait_set(rcl_wait_set_t * wait_set);
 
+  /// Set a callback to be called whenever the guard condition is triggered.
+  /**
+   * The callback receives a size_t which is the number of times the guard condition was triggered
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if the guard condition was triggered 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 guard condition
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called when the guard condition is triggered
+   */
   RCLCPP_PUBLIC
   void
   set_on_trigger_callback(std::function<void(size_t)> callback);

rclcpp/include/rclcpp/logger.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -122,6 +123,7 @@ private:
   : name_(new std::string(name)) {}
 
   std::shared_ptr<const std::string> name_;
+  std::shared_ptr<std::pair<std::string, std::string>> logger_sublogger_pairname_ = nullptr;
 
 public:
   RCLCPP_PUBLIC
@@ -157,13 +159,7 @@ public:
    */
   RCLCPP_PUBLIC
   Logger
-  get_child(const std::string & suffix)
-  {
-    if (!name_) {
-      return Logger();
-    }
-    return Logger(*name_ + "." + suffix);
-  }
+  get_child(const std::string & suffix);
 
   /// Set level for current logger.
   /**
@@ -174,6 +170,24 @@ public:
   RCLCPP_PUBLIC
   void
   set_level(Level level);
+
+  /// Get effective level for current logger.
+  /**
+   * The effective level is determined as the severity level of
+   * the logger if it is set, otherwise it is the first specified severity
+   * level of the logger's ancestors, starting with its closest ancestor.
+   * The ancestor hierarchy is signified by logger names being separated by dots:
+   * a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are
+   * ancestors of `x.y.z`, etc.
+   * If the level has not been set for the logger nor any of its
+   * ancestors, the default level is used.
+   *
+   * \throws rclcpp::exceptions::RCLError if any error happens.
+   * \return Level for the current logger.
+   */
+  RCLCPP_PUBLIC
+  Level
+  get_effective_level() const;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node.hpp

@@ -227,7 +227,7 @@ public:
     )
   );
 
-  /// Create a timer.
+  /// Create a wall timer that uses the wall clock to drive the callback.
   /**
    * \param[in] period Time interval between triggers of the callback.
    * \param[in] callback User-defined callback function.
@@ -240,18 +240,47 @@ public:
     CallbackT callback,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create a timer that uses the node clock to drive the callback.
+  /**
+   * \param[in] period Time interval between triggers of the callback.
+   * \param[in] callback User-defined callback function.
+   * \param[in] group Callback group to execute this timer's callback in.
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
+  typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+  create_timer(
+    std::chrono::duration<DurationRepT, DurationT> period,
+    CallbackT callback,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
   /// Create and return a Client.
   /**
    * \param[in] service_name The topic to service on.
    * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
    * \param[in] group Callback group to call the service.
    * \return Shared pointer to the created client.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  template<typename ServiceT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  typename rclcpp::Client<ServiceT>::SharedPtr
+  create_client(
+    const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  /// Create and return a Client.
+  /**
+   * \param[in] service_name The name on which the service is accessible.
+   * \param[in] qos Quality of service profile for client.
+   * \param[in] group Callback group to handle the reply to service calls.
+   * \return Shared pointer to the created client.
    */
   template<typename ServiceT>
   typename rclcpp::Client<ServiceT>::SharedPtr
   create_client(
     const std::string & service_name,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Create and return a Service.
@@ -261,13 +290,31 @@ public:
    * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
    * \param[in] group Callback group to call the service.
    * \return Shared pointer to the created service.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  template<typename ServiceT, typename CallbackT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  typename rclcpp::Service<ServiceT>::SharedPtr
+  create_service(
+    const std::string & service_name,
+    CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  /// Create and return a Service.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos Quality of service profile for the service.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
    */
   template<typename ServiceT, typename CallbackT>
   typename rclcpp::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
     CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Create and return a GenericPublisher.
@@ -336,11 +383,21 @@ public:
    *
    * If `ignore_override` is `true`, the parameter override will be ignored.
    *
-   * This method, if successful, will result in any callback registered with
-   * add_on_set_parameters_callback to be called.
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
+   * to be called for the parameter being set.
+   *
+   * If a callback was registered previously with `add_on_set_parameters_callback`,
+   * it will be called prior to setting the parameter for the node.
    * If that callback prevents the initial value for the parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameter successfully for the node.
+   *
+   * This method will _not_ result in any callbacks registered with
+   * `add_pre_set_parameters_callback` to be called.
+   *
    * The returned reference will remain valid until the parameter is
    * undeclared.
    *
@@ -457,11 +514,22 @@ public:
    * If `ignore_overrides` is `true`, all the overrides of the parameters declared
    * by the function call will be ignored.
    *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
+   * to be called once for each parameter.
+   *
    * This method, if successful, will result in any callback registered with
-   * add_on_set_parameters_callback to be called, once for each parameter.
+   * `add_on_set_parameters_callback` to be called, once for each parameter.
    * If that callback prevents the initial value for any parameter from being
    * set then rclcpp::exceptions::InvalidParameterValueException is thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameters successfully for the node,
+   * once for each parameter.
+   *
+   * This method will _not_ result in any callbacks registered with
+   * `add_pre_set_parameters_callback` to be called.
+   *
    * \param[in] namespace_ The namespace in which to declare the parameters.
    * \param[in] parameters The parameters to set in the given namespace.
    * \param[in] ignore_overrides When `true`, the parameters overrides are ignored.
@@ -499,8 +567,9 @@ public:
 
   /// Undeclare a previously declared parameter.
   /**
-   * This method will not cause a callback registered with
-   * add_on_set_parameters_callback to be called.
+   * This method will _not_ cause a callback registered with any of the
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called.
    *
    * \param[in] name The name of the parameter to be undeclared.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
@@ -534,11 +603,24 @@ public:
    * Parameter overrides are ignored by set_parameter.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called.
+   * `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called once for the parameter
+   * being set.
+   *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` to be called.
    * If the callback prevents the parameter from being set, then it will be
    * reflected in the SetParametersResult that is returned, but no exception
    * will be thrown.
    *
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called once prior to the validation of the parameter for the node.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called once after setting the parameter successfully for the node.
+   *
    * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the
    * existing parameter type is something else, then the parameter will be
    * implicitly undeclared.
@@ -575,11 +657,25 @@ public:
    * corresponding SetParametersResult in the vector returned by this function.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called, once for each parameter.
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called once for each parameter.
+
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called prior to the validation of parameters for the node,
+   * once for each parameter.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * This method will result in any callback registered with
+   * `add_on_set_parameters_callback` to be called, once for each parameter.
    * If the callback prevents the parameter from being set, then, as mentioned
    * before, it will be reflected in the corresponding SetParametersResult
    * that is returned, but no exception will be thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the parameters successfully for the node,
+   * once for each parameter.
+   *
    * Like set_parameter() this method will implicitly undeclare parameters
    * with the type rclcpp::PARAMETER_NOT_SET.
    *
@@ -606,11 +702,25 @@ public:
    * If the exception is thrown then none of the parameters will have been set.
    *
    * This method will result in any callback registered with
-   * add_on_set_parameters_callback to be called, just one time.
+   * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
+   * `add_post_set_parameters_callback` to be called only 'once' for all parameters.
+   *
+   * If a callback was registered previously with `add_pre_set_parameters_callback`,
+   * it will be called prior to the validation of node parameters, just one time
+   * for all parameters.
+   * If this callback makes modified parameter list empty, then it will be reflected
+   * in the returned result; no exceptions will be raised in this case.
+   *
+   * This method will result in any callback registered with
+   * 'add_on_set_parameters_callback' to be called, just one time.
    * If the callback prevents the parameters from being set, then it will be
    * reflected in the SetParametersResult which is returned, but no exception
    * will be thrown.
    *
+   * If a callback was registered previously with `add_post_set_parameters_callback`,
+   * it will be called after setting the node parameters successfully, just one time
+   * for all parameters.
+   *
    * If you pass multiple rclcpp::Parameter instances with the same name, then
    * only the last one in the vector (forward iteration) will be set.
    *
@@ -630,17 +740,21 @@ public:
   /**
    * If the parameter has not been declared, then this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception.
+   * If the parameter has not been initialized, then this method may throw the
+   * rclcpp::exceptions::ParameterUninitializedException exception.
    *
    * If undeclared parameters are allowed, see the node option
    * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will
-   * not throw an exception, and instead return a default initialized
-   * rclcpp::Parameter, which has a type of
+   * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception,
+   * and instead return a default initialized rclcpp::Parameter, which has a type of
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
    *
    * \param[in] name The name of the parameter to get.
    * \return The requested parameter inside of a rclcpp parameter object.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
    *   has not been declared and undeclared parameters are not allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter
+   *   has not been initialized.
    */
   RCLCPP_PUBLIC
   rclcpp::Parameter
@@ -724,12 +838,12 @@ public:
 
   /// Return the parameters by the given parameter names.
   /**
-   * Like get_parameters(), this method may throw the
+   * Like get_parameter(const std::string &), this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception if the
    * requested parameter has not been declared and undeclared parameters are
-   * not allowed.
+   * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception.
    *
-   * Also like get_parameters(), if undeclared parameters are allowed and the
+   * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the
    * parameter has not been declared, then the corresponding rclcpp::Parameter
    * will be default initialized and therefore have the type
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
@@ -739,6 +853,8 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the
    *   parameters have not been declared and undeclared parameters are not
    *   allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if any of the
+   *   parameters have not been initialized.
    */
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>
@@ -859,12 +975,85 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  using PreSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PreSetParametersCallbackHandle;
+  using PreSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType;
+
   using OnSetParametersCallbackHandle =
     rclcpp::node_interfaces::OnSetParametersCallbackHandle;
-  using OnParametersSetCallbackType =
-    rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
+  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
+    OnSetParametersCallbackType;
 
-  /// Add a callback for when parameters are being set.
+  using PostSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PostSetParametersCallbackHandle;
+  using PostSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType;
+
+  /// Add a callback that gets triggered before parameters are validated.
+  /**
+   * This callback can be used to modify the original list of parameters being
+   * set by the user.
+   *
+   * The modified list of parameters is then forwarded to the "on set parameter"
+   * callback for validation.
+   *
+   * The callback is called whenever any of the `set_parameter*` methods are called
+   * or when a set parameter service request is received.
+   *
+   * The callback takes a reference to the vector of parameters to be set.
+   *
+   * The vector of parameters may be modified by the callback.
+   *
+   * One of the use case of "pre set callback" can be updating additional parameters
+   * conditioned on changes to a parameter.
+   *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
+   * For an example callback:
+   *
+   *```cpp
+   * void
+   * preSetParameterCallback(std::vector<rclcpp::Parameter> & parameters)
+   * {
+   *  for (auto & param : parameters) {
+   *    if (param.get_name() == "param1") {
+   *      parameters.push_back(rclcpp::Parameter("param2", 4.0));
+   *    }
+   *  }
+   * }
+   * ```
+   * The above callback appends 'param2' to the list of parameters to be set if
+   * 'param1' is being set by the user.
+   *
+   * All parameters in the vector will be set atomically.
+   *
+   * Note that the callback is only called while setting parameters with `set_parameter`,
+   * `set_parameters`, `set_parameters_atomically`, or externally with a parameters service.
+   *
+   * The callback is not called when parameters are declared with `declare_parameter`
+   * or `declare_parameters`.
+   *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
+   * An empty modified parameter list from the callback will result in "set_parameter*"
+   * returning an unsuccessful result.
+   *
+   * The `remove_pre_set_parameters_callback` can be used to deregister the callback.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback);
+
+  /// Add a callback to validate parameters before they are set.
   /**
    * The callback signature is designed to allow handling of any of the above
    * `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@@ -872,6 +1061,9 @@ public:
    * rcl_interfaces::msg::SetParametersResult to indicate whether or not the
    * parameter should be set or not, and if not why.
    *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
    * For an example callback:
    *
    * ```cpp
@@ -903,6 +1095,8 @@ public:
    * this callback, so when checking a new value against the existing one, you
    * must account for the case where the parameter is not yet set.
    *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
    * Some constraints like read_only are enforced before the callback is called.
    *
    * The callback may introspect other already set parameters (by calling any
@@ -931,7 +1125,80 @@ public:
   RCLCPP_PUBLIC
   RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback);
+
+  /// Add a callback that gets triggered after parameters are set successfully.
+  /**
+   * The callback is called when any of the `set_parameter*` or `declare_parameter*`
+   * methods are successful.
+   *
+   * Users should retain a copy of the returned shared pointer, as the callback
+   * is valid only as long as the smart pointer is alive.
+   *
+   * The callback takes a reference to a const vector of parameters that have been
+   * set successfully.
+   *
+   * The post callback can be valuable as a place to cause side-effects based on
+   * parameter changes.
+   * For instance updating internally tracked class attributes once parameters
+   * have been changed successfully.
+   *
+   * For an example callback:
+   *
+   * ```cpp
+   * void
+   * postSetParameterCallback(const std::vector<rclcpp::Parameter> & parameters)
+   * {
+   *  for(const auto & param:parameters) {
+   *   // the internal class member can be changed after
+   *   // successful change to param1 or param2
+   *    if(param.get_name() == "param1") {
+   *      internal_tracked_class_parameter_1_ = param.get_value<double>();
+   *    }
+   *    else if(param.get_name() == "param2") {
+   *      internal_tracked_class_parameter_2_ = param.get_value<double>();
+   *    }
+   *  }
+   * }
+   * ```
+   *
+   * The above callback takes a const reference to list of parameters that have been
+   * set successfully and as a result of this updates the internally tracked class attributes
+   * `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_`
+   * respectively.
+   *
+   * This callback should not modify parameters.
+   *
+   * The callback is called when parameters are declared with `declare_parameter`
+   * or `declare_parameters`. See `declare_parameter` or `declare_parameters` above.
+   *
+   * The callback is not called when parameters are undeclared with `undeclare_parameter`.
+   *
+   * If you want to make changes to parameters based on changes to another, use
+   * `add_pre_set_parameters_callback`.
+   *
+   * The `remove_post_set_parameters_callback` can be used to deregister the callback.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback);
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_pre_set_parameters_callback`.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler);
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -960,6 +1227,18 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
 
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_post_set_parameters_callback`.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler);
+
   /// Get the fully-qualified names of all available nodes.
   /**
    * The fully-qualified name includes the local namespace and name of the node.

rclcpp/include/rclcpp/node_impl.hpp

@@ -120,6 +120,38 @@ Node::create_wall_timer(
     this->node_timers_.get());
 }
 
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::GenericTimer<CallbackT>::SharedPtr
+Node::create_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_timer(
+    this->get_clock(),
+    period,
+    std::move(callback),
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get());
+}
+
+template<typename ServiceT>
+typename Client<ServiceT>::SharedPtr
+Node::create_client(
+  const std::string & service_name,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_client<ServiceT>(
+    node_base_,
+    node_graph_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    qos,
+    group);
+}
+
 template<typename ServiceT>
 typename Client<ServiceT>::SharedPtr
 Node::create_client(
@@ -136,6 +168,23 @@ Node::create_client(
     group);
 }
 
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+Node::create_service(
+  const std::string & service_name,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos,
+    group);
+}
+
 template<typename ServiceT, typename CallbackT>
 typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(

rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp

@@ -0,0 +1,208 @@
+// 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))
+  {}
+
+  NodeInterfacesStorage()
+  : interfaces_()
+  {}
+
+  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>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,7 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
-#include <functional>
+#include <atomic>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -39,6 +39,13 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<N
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
 
+  /// Constructor.
+  /**
+   * If nullptr (default) is given for the default_callback_group, one will
+   * be created by the constructor using the create_callback_group() method,
+   * but virtual dispatch will not occur so overrides of that method will not
+   * be used.
+   */
   RCLCPP_PUBLIC
   NodeBase(
     const std::string & node_name,
@@ -46,7 +53,8 @@ public:
     rclcpp::Context::SharedPtr context,
     const rcl_node_options_t & rcl_node_options,
     bool use_intra_process_default,
-    bool enable_topic_statistics_default);
+    bool enable_topic_statistics_default,
+    rclcpp::CallbackGroup::SharedPtr default_callback_group = nullptr);
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -15,10 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 
+#include <atomic>
+#include <functional>
 #include <memory>
-#include <mutex>
 #include <string>
-#include <vector>
 
 #include "rcl/node.h"
 
@@ -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_

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 
+#include "rcl/time.h"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -42,7 +43,8 @@ public:
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rcl_clock_type_t clock_type);
 
   RCLCPP_PUBLIC
   virtual
@@ -67,7 +69,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
-  rclcpp::Clock::SharedPtr ros_clock_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace node_interfaces

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_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 
+#include <atomic>
 #include <chrono>
+#include <condition_variable>
 #include <map>
 #include <memory>
 #include <mutex>

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"
 
@@ -382,4 +383,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_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -0,0 +1,164 @@
+// 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)
+  {}
+
+  // Create a NodeInterfaces object with no bound interfaces
+  NodeInterfaces()
+  : NodeInterfacesSupportsT()
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_

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
@@ -37,14 +38,18 @@ public:
   ~NodeLoggingInterface() = default;
 
   /// Return the logger of the node.
-  /** \return The logger of the node. */
+  /**
+   * \return The logger of the node.
+   */
   RCLCPP_PUBLIC
   virtual
   rclcpp::Logger
   get_logger() const = 0;
 
   /// Return the logger name associated with the node.
-  /** \return The logger name associated with the node. */
+  /**
+   * \return The logger name associated with the node.
+   */
   RCLCPP_PUBLIC
   virtual
   const char *
@@ -54,4 +59,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_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -15,9 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
+#include <list>
 #include <map>
 #include <memory>
-#include <list>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -84,6 +85,16 @@ class NodeParameters : public NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
 
+  /// Constructor.
+  /**
+   * If using automatically_declare_parameters_from_overrides, overrides of
+   * get_parameter_overrides(), has_parameter(), declare_parameter() will not
+   * be respected.
+   * If this is an issue, pass false for
+   * automatically_declare_parameters_from_overrides and invoke
+   * perform_automatically_declare_parameters_from_overrides() manually after
+   * construction.
+   */
   RCLCPP_PUBLIC
   NodeParameters(
     const node_interfaces::NodeBaseInterface::SharedPtr node_base,
@@ -171,20 +182,48 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override;
+
   RCLCPP_PUBLIC
   RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) override;
 
   RCLCPP_PUBLIC
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
+  RCLCPP_PUBLIC
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler)
+  override;
+
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override;
+
   RCLCPP_PUBLIC
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
-  using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PreSetCallbacksHandleContainer = std::list<PreSetParametersCallbackHandle::WeakPtr>;
+  using OnSetCallbacksHandleContainer = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PostSetCallbacksHandleContainer = std::list<PostSetParametersCallbackHandle::WeakPtr>;
+  using CallbacksContainerType [[deprecated("use OnSetCallbacksHandleContainer instead")]] =
+    OnSetCallbacksHandleContainer;
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  perform_automatically_declare_parameters_from_overrides();
 
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
@@ -196,9 +235,11 @@ private:
   // declare_parameter, etc).  In those cases, this will be set to false.
   bool parameter_modification_enabled_{true};
 
-  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
+  PreSetCallbacksHandleContainer pre_set_parameters_callback_container_;
 
-  CallbacksContainerType on_parameters_set_callback_container_;
+  OnSetCallbacksHandleContainer on_set_parameters_callback_container_;
+
+  PostSetCallbacksHandleContainer post_set_parameters_callback_container_;
 
   std::map<std::string, ParameterInfo> parameters_;
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,8 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <functional>
 #include <map>
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -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"
 
@@ -33,16 +34,38 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+struct PreSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle)
+
+  using PreSetParametersCallbackType =
+    std::function<void (std::vector<rclcpp::Parameter> &)>;
+
+  PreSetParametersCallbackType callback;
+};
+
 struct OnSetParametersCallbackHandle
 {
   RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
 
-  using OnParametersSetCallbackType =
+  using OnSetParametersCallbackType =
     std::function<
     rcl_interfaces::msg::SetParametersResult(
       const std::vector<rclcpp::Parameter> &)>;
+  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
+    OnSetParametersCallbackType;
 
-  OnParametersSetCallbackType callback;
+  OnSetParametersCallbackType callback;
+};
+
+struct PostSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle)
+
+  using PostSetParametersCallbackType =
+    std::function<void (const std::vector<rclcpp::Parameter> &)>;
+
+  PostSetParametersCallbackType callback;
 };
 
 /// Pure virtual interface class for the NodeParameters part of the Node API.
@@ -185,16 +208,46 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType;
+  using PostSetParametersCallbackType =
+    PostSetParametersCallbackHandle::PostSetParametersCallbackType;
+  using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType;
 
-  /// Add a callback for when parameters are being set.
+  /// Add a callback that gets triggered before parameters are validated.
+  /**
+   * \sa rclcpp::Node::add_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0;
+
+  /// Add a callback to validate parameters before they are set.
   /**
    * \sa rclcpp::Node::add_on_set_parameters_callback
    */
   RCLCPP_PUBLIC
   virtual
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0;
+
+  /// Add a callback that gets triggered after parameters are set successfully.
+  /**
+   * \sa rclcpp::Node::add_post_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0;
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0;
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -205,6 +258,15 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
 
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_post_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0;
+
   /// Return the initial parameter values used by the NodeParameters to override default values.
   RCLCPP_PUBLIC
   virtual
@@ -215,4 +277,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_

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_

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_

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_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -15,8 +15,6 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 
-#include <functional>
-#include <memory>
 #include <string>
 
 #include "rcl/publisher.h"
@@ -24,6 +22,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 +96,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_

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_

rclcpp/include/rclcpp/node_options.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "rcl/time.h"
 #include "rcl/node_options.h"
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -46,6 +47,7 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_type = RCL_ROS_TIME
    *   - clock_qos = rclcpp::ClockQoS()
    *   - use_clock_thread = true
    *   - rosout_qos = rclcpp::RosoutQoS()
@@ -246,6 +248,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock type.
+  RCLCPP_PUBLIC
+  const rcl_clock_type_t &
+  clock_type() const;
+
+  /// Set the clock type.
+  /**
+   * The clock type to be used by the node.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_type(const rcl_clock_type_t & clock_type);
+
   /// Return a reference to the clock QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -400,6 +415,8 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rcl_clock_type_t clock_type_ {RCL_ROS_TIME};
+
   rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
 
   bool use_clock_thread_ {true};

rclcpp/include/rclcpp/parameter_client.hpp

@@ -39,6 +39,7 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/parameter_map.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/rmw.h"
@@ -51,6 +52,37 @@ class AsyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
+  /// Create an async parameters client.
+  /**
+   * \param[in] node_base_interface The node base interface of the corresponding node.
+   * \param[in] node_topics_interface Node topic base interface.
+   * \param[in] node_graph_interface The node graph interface of the corresponding node.
+   * \param[in] node_services_interface Node service interface.
+   * \param[in] remote_node_name Name of the remote node
+   * \param[in] qos_profile The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node_base_interface,
+      node_topics_interface,
+      node_graph_interface,
+      node_services_interface,
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
+      group)
+  {}
+
   /// Create an async parameters client.
   /**
    * \param[in] node_base_interface The node base interface of the corresponding node.
@@ -58,7 +90,7 @@ public:
    * \param[in] node_graph_interface The node graph interface of the corresponding node.
    * \param[in] node_services_interface Node service interface.
    * \param[in] remote_node_name (optional) name of the remote node
-   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] qos_profile (optional) The qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   RCLCPP_PUBLIC
@@ -68,21 +100,45 @@ public:
     const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
     const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
   /// Constructor
   /**
-   * \param[in] node The async paramters client will be added to this node.
+   * \param[in] node The async parameters client will be added to this node.
+   * \param[in] remote_node_name name of the remote node
+   * \param[in] qos_profile The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  AsyncParametersClient(
+    const std::shared_ptr<NodeT> node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
+      group)
+  {}
+
+  /**
+   * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
-   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] qos_profile (optional) The qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
   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,
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr)
   : AsyncParametersClient(
       node->get_node_base_interface(),
@@ -96,16 +152,40 @@ public:
 
   /// Constructor
   /**
-   * \param[in] node The  async paramters client will be added to this node.
+   * \param[in] node The async parameters client will be added to this node.
+   * \param[in] remote_node_name Name of the remote node
+   * \param[in] qos_profile The rmw qos profile to use to subscribe
+   * \param[in] group (optional) The async parameter client will be added to this callback group.
+   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
+   */
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  AsyncParametersClient(
+    NodeT * node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  : AsyncParametersClient(
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
+      group)
+  {}
+
+  /**
+   * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
-   * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe
+   * \param[in] qos_profile (optional) The qos profile to use to subscribe
    * \param[in] group (optional) The async parameter client will be added to this callback group.
    */
   template<typename NodeT>
   explicit AsyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr)
   : AsyncParametersClient(
       node->get_node_base_interface(),
@@ -185,6 +265,9 @@ public:
   /**
    * This function filters the parameters to be set based on the node name.
    *
+   * If two duplicate keys exist in node names belongs to one FQN, there is no guarantee
+   * which one could be set.
+   *
    * \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
@@ -208,9 +291,7 @@ public:
   typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
   on_parameter_event(
     CallbackT && callback,
-    const rclcpp::QoS & qos = (
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
-    ),
+    const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
   ))
@@ -235,9 +316,7 @@ public:
   on_parameter_event(
     NodeT && node,
     CallbackT && callback,
-    const rclcpp::QoS & qos = (
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
-    ),
+    const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
   ))
@@ -304,11 +383,24 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  SyncParametersClient(
+    std::shared_ptr<NodeT> node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
+  {}
+
   template<typename NodeT>
   explicit SyncParametersClient(
     std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
   : SyncParametersClient(
       std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
       node,
@@ -316,12 +408,29 @@ public:
       qos_profile)
   {}
 
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  SyncParametersClient(
+    rclcpp::Executor::SharedPtr executor,
+    std::shared_ptr<NodeT> node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
+  {}
+
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
     std::shared_ptr<NodeT> node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
   : SyncParametersClient(
       executor,
       node->get_node_base_interface(),
@@ -332,11 +441,24 @@ public:
       qos_profile)
   {}
 
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  SyncParametersClient(
+    NodeT * node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile)
+  : SyncParametersClient(
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+      node,
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
+  {}
+
   template<typename NodeT>
   explicit SyncParametersClient(
     NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
   : SyncParametersClient(
       std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
       node,
@@ -344,12 +466,29 @@ public:
       qos_profile)
   {}
 
+  template<typename NodeT>
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  SyncParametersClient(
+    rclcpp::Executor::SharedPtr executor,
+    NodeT * node,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile)
+  : SyncParametersClient(
+      executor,
+      node->get_node_base_interface(),
+      node->get_node_topics_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
+  {}
+
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
     NodeT * node,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
   : SyncParametersClient(
       executor,
       node->get_node_base_interface(),
@@ -360,6 +499,28 @@ public:
       qos_profile)
   {}
 
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::Executor::SharedPtr executor,
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name,
+    const rmw_qos_profile_t & qos_profile)
+  : executor_(executor), node_base_interface_(node_base_interface)
+  {
+    async_parameters_client_ =
+      std::make_shared<AsyncParametersClient>(
+      node_base_interface,
+      node_topics_interface,
+      node_graph_interface,
+      node_services_interface,
+      remote_node_name,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)));
+  }
+
   RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
@@ -368,7 +529,7 @@ public:
     const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
     const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
     const std::string & remote_node_name = "",
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters)
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
   : executor_(executor), node_base_interface_(node_base_interface)
   {
     async_parameters_client_ =

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -86,6 +86,9 @@ struct ParameterEventCallbackHandle
  * the ROS node supplied in the ParameterEventHandler constructor.
  * The callback, a lambda function in this case, simply prints out the value of the parameter.
  *
+ * Note: the object returned from add_parameter_callback must be captured or the callback will
+ * be immediately unregistered.
+ *
  * You may also monitor for changes to parameters in other nodes by supplying the node
  * name to add_parameter_callback:
  *
@@ -103,8 +106,8 @@ struct ParameterEventCallbackHandle
  * In this case, the callback will be invoked whenever "some_remote_param_name" changes
  * on remote node "some_remote_node_name".
  *
- * To remove a parameter callback, call remove_parameter_callback, passing the handle returned
- * from add_parameter_callback:
+ * To remove a parameter callback, reset the callback handle smart pointer or call
+ * remove_parameter_callback, passing the handle returned from add_parameter_callback:
  *
  *   param_handler->remove_parameter_callback(handle2);
  *
@@ -152,9 +155,12 @@ struct ParameterEventCallbackHandle
  * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added,
  * the callbacks are invoked last-in, first-called order (LIFO).
  *
- * To remove a parameter event callback, use:
+ * Note: the callback handle returned from add_parameter_event_callback must be captured or
+ * the callback will immediately be unregistered.
  *
- *   param_handler->remove_event_parameter_callback(handle);
+ * To remove a parameter event callback, reset the callback smart pointer or use:
+ *
+ *   param_handler->remove_event_parameter_callback(handle3);
  */
 class ParameterEventHandler
 {
@@ -189,10 +195,14 @@ public:
   /**
    * This function may be called multiple times to set multiple parameter event callbacks.
    *
+   * Note: if the returned callback handle smart pointer is not captured, the callback is
+   * immediatedly unregistered. A compiler warning should be generated to warn of this.
+   *
    * \param[in] callback Function callback to be invoked on parameter updates.
    * \returns A handle used to refer to the callback.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   ParameterEventCallbackHandle::SharedPtr
   add_parameter_event_callback(
     ParameterEventCallbackType callback);
@@ -212,12 +222,17 @@ public:
   /**
    * If a node_name is not provided, defaults to the current node.
    *
+   * Note: if the returned callback handle smart pointer is not captured, the callback
+   * is immediately unregistered. A compiler warning should be generated to warn
+   * of this.
+   *
    * \param[in] parameter_name Name of parameter to monitor.
    * \param[in] callback Function callback to be invoked upon parameter update.
    * \param[in] node_name Name of node which hosts the parameter.
    * \returns A handle used to refer to the callback.
    */
   RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
   ParameterCallbackHandle::SharedPtr
   add_parameter_callback(
     const std::string & parameter_name,
@@ -268,6 +283,7 @@ public:
    * \param[in] parameter_name Name of parameter.
    * \param[in] node_name Name of node which hosts the parameter.
    * \returns The resultant rclcpp::Parameter from the event.
+   * \throws std::runtime_error if input node name doesn't match the node name in parameter event.
    */
   RCLCPP_PUBLIC
   static rclcpp::Parameter

rclcpp/include/rclcpp/parameter_map.hpp

@@ -35,11 +35,13 @@ using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
 
 /// 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);
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn = nullptr);
 
 /// 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.
@@ -51,11 +53,20 @@ parameter_value_from(const rcl_variant_t * const c_value);
 
 /// Get the ParameterMap from a yaml file.
 /// \param[in] yaml_filename full name of the yaml file.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
 /// \returns an instance of a parameter map
 /// \throws from rcl error of rcl_parse_yaml_file()
 RCLCPP_PUBLIC
 ParameterMap
-parameter_map_from_yaml_file(const std::string & yaml_filename);
+parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr);
+
+/// Get the Parameters from ParameterMap.
+/// \param[in] parameter_map a parameter map.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+/// \returns a list of a parameter
+RCLCPP_PUBLIC
+std::vector<Parameter>
+parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr);
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/parameter_service.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/rmw.h"
 
@@ -39,12 +40,26 @@ class ParameterService
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(ParameterService)
 
+  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
   RCLCPP_PUBLIC
-  explicit ParameterService(
+  ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
     const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
     rclcpp::node_interfaces::NodeParametersInterface * node_params,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+    const rmw_qos_profile_t & qos_profile)
+  : ParameterService(
+      node_base,
+      node_services,
+      node_params,
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
+  {}
+
+  RCLCPP_PUBLIC
+  ParameterService(
+    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    rclcpp::node_interfaces::NodeParametersInterface * node_params,
+    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS());
 
 private:
   rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;

rclcpp/include/rclcpp/publisher.hpp

@@ -131,40 +131,16 @@ public:
       node_base,
       topic,
       rclcpp::get_message_type_support_handle<MessageT>(),
-      options.template to_rcl_publisher_options<MessageT>(qos)),
+      options.template to_rcl_publisher_options<MessageT>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     options_(options),
     published_type_allocator_(*options.get_allocator()),
     ros_message_type_allocator_(*options.get_allocator())
   {
     allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_);
     allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
-
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
     // Setup continues in the post construction method, post_init_setup().
   }
 
@@ -405,10 +381,6 @@ public:
     if (!loaned_msg.is_valid()) {
       throw std::runtime_error("loaned message is not valid");
     }
-    if (intra_process_is_enabled_) {
-      // TODO(Karsten1987): support loaned message passed by intraprocess
-      throw std::runtime_error("storing loaned messages in intra process is not supported yet");
-    }
 
     // verify that publisher supports loaned messages
     // TODO(Karsten1987): This case separation has to be done in rclcpp
@@ -422,7 +394,7 @@ public:
     } else {
       // we don't release the ownership, let the middleware copy the ros message
       // and thus the destructor of rclcpp::LoanedMessage cleans up the memory.
-      this->do_inter_process_publish(loaned_msg.get());
+      this->publish(loaned_msg.get());
     }
   }
 

rclcpp/include/rclcpp/publisher_base.hpp

@@ -33,7 +33,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rcpputils/time.hpp"
@@ -78,11 +78,18 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
     const rosidl_message_type_support_t & type_support,
-    const rcl_publisher_options_t & publisher_options);
+    const rcl_publisher_options_t & publisher_options,
+    const PublisherEventCallbacks & event_callbacks,
+    bool use_default_callbacks);
 
   RCLCPP_PUBLIC
   virtual ~PublisherBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this publisher publishes on.
   /** \return The topic name. */
   RCLCPP_PUBLIC
@@ -117,7 +124,7 @@ public:
   /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
   const
-  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get subscription count
@@ -269,7 +276,7 @@ public:
    * 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
+   * \sa rclcpp::EventHandlerBase::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
@@ -320,7 +327,7 @@ protected:
     const EventCallbackT & callback,
     const rcl_publisher_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+    auto handler = std::make_shared<EventHandler<EventCallbackT,
         std::shared_ptr<rcl_publisher_t>>>(
       callback,
       rcl_publisher_event_init,
@@ -332,12 +339,15 @@ protected:
   RCLCPP_PUBLIC
   void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
 
+  RCLCPP_PUBLIC
+  void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
+
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
   std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
   std::unordered_map<rcl_publisher_event_type_t,
-    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+    std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
     std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
@@ -348,6 +358,8 @@ protected:
   rmw_gid_t rmw_gid_;
 
   const rosidl_message_type_support_t type_support_;
+
+  const PublisherEventCallbacks event_callbacks_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_options.hpp

@@ -26,7 +26,7 @@
 #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
 
 namespace rclcpp
@@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   /// Optional custom allocator.
   std::shared_ptr<Allocator> allocator = nullptr;
 
-  PublisherOptionsWithAllocator<Allocator>() {}
+  PublisherOptionsWithAllocator() {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)

rclcpp/include/rclcpp/qos.hpp

@@ -44,6 +44,7 @@ enum class ReliabilityPolicy
   BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
   Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE,
   SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN,
 };
 
@@ -52,6 +53,7 @@ enum class DurabilityPolicy
   Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE,
   TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
   SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN,
 };
 
@@ -60,6 +62,7 @@ enum class LivelinessPolicy
   Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
   ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
   SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  BestAvailable = RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE,
   Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN,
 };
 
@@ -77,7 +80,9 @@ struct RCLCPP_PUBLIC QoSInitialization
   size_t depth;
 
   /// Constructor which takes both a history policy and a depth (even if it would be unused).
-  QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg);
+  QoSInitialization(
+    rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
+    bool print_depth_warning = true);
 
   /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth.
   static
@@ -94,7 +99,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization
 /// Use to initialize the QoS with the keep_last history setting and the given depth.
 struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
 {
-  explicit KeepLast(size_t depth);
+  explicit KeepLast(size_t depth, bool print_depth_warning = true);
 };
 
 /// Encapsulation of Quality of Service settings.
@@ -180,6 +185,10 @@ public:
   QoS &
   best_effort();
 
+  /// Set the reliability setting to best available.
+  QoS &
+  reliability_best_available();
+
   /// Set the durability setting.
   QoS &
   durability(rmw_qos_durability_policy_t durability);
@@ -199,6 +208,10 @@ public:
   QoS &
   transient_local();
 
+  /// Set the durability setting to best available.
+  QoS &
+  durability_best_available();
+
   /// Set the deadline setting.
   QoS &
   deadline(rmw_time_t deadline);
@@ -488,6 +501,36 @@ public:
   ));
 };
 
+/**
+ * Best available QoS class
+ *
+ * Match majority of endpoints currently available while maintaining the highest level of service.
+ * Policies are chosen at the time of creating a subscription or publisher.
+ * The middleware is not expected to update policies after creating a subscription or publisher,
+ * even if one or more policies are incompatible with newly discovered endpoints.
+ * Therefore, this profile should be used with care since non-deterministic behavior can occur due
+ * to races with discovery.
+ *
+ *    - History: Keep last,
+ *    - Depth: 10,
+ *    - Reliability: Best available,
+ *    - Durability: Best available,
+ *    - Deadline: Best available,
+ *    - Lifespan: Default,
+ *    - Liveliness: Best available,
+ *    - Liveliness lease duration: Best available,
+ *    - avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC BestAvailableQoS : public QoS
+{
+public:
+  explicit
+  BestAvailableQoS(
+    const QoSInitialization & qos_initialization = (
+      QoSInitialization::from_rmw(rmw_qos_profile_best_available)
+  ));
+};
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__QOS_HPP_

rclcpp/include/rclcpp/qos_event.hpp

@@ -15,277 +15,8 @@
 #ifndef RCLCPP__QOS_EVENT_HPP_
 #define RCLCPP__QOS_EVENT_HPP_
 
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <stdexcept>
-#include <string>
+#warning This header is obsolete, please include rclcpp/event_handler.hpp instead
 
-#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
-{
-
-using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
-using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
-using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
-using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
-using QOSMessageLostInfo = rmw_message_lost_status_t;
-using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
-using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
-
-using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
-using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
-using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
-using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
-using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
-using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
-using QOSRequestedIncompatibleQoSCallbackType =
-  std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
-
-/// Contains callbacks for various types of events a Publisher can receive from the middleware.
-struct PublisherEventCallbacks
-{
-  QOSDeadlineOfferedCallbackType deadline_callback;
-  QOSLivelinessLostCallbackType liveliness_callback;
-  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
-};
-
-/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
-struct SubscriptionEventCallbacks
-{
-  QOSDeadlineRequestedCallbackType deadline_callback;
-  QOSLivelinessChangedCallbackType liveliness_callback;
-  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
-  QOSMessageLostCallbackType message_lost_callback;
-};
-
-class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
-{
-public:
-  RCLCPP_PUBLIC
-  UnsupportedEventTypeException(
-    rcl_ret_t ret,
-    const rcl_error_state_t * error_state,
-    const std::string & prefix);
-
-  RCLCPP_PUBLIC
-  UnsupportedEventTypeException(
-    const exceptions::RCLErrorBase & base_exc,
-    const std::string & prefix);
-};
-
-class QOSEventHandlerBase : public Waitable
-{
-public:
-  enum class EntityType : std::size_t
-  {
-    Event,
-  };
-
-  RCLCPP_PUBLIC
-  virtual ~QOSEventHandlerBase();
-
-  /// Get the number of ready events
-  RCLCPP_PUBLIC
-  size_t
-  get_number_of_ready_events() override;
-
-  /// Add the Waitable to a wait set.
-  RCLCPP_PUBLIC
-  void
-  add_to_wait_set(rcl_wait_set_t * wait_set) override;
-
-  /// Check if the Waitable is ready.
-  RCLCPP_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>
-class QOSEventHandler : public QOSEventHandlerBase
-{
-public:
-  template<typename InitFuncT, typename EventTypeEnum>
-  QOSEventHandler(
-    const EventCallbackT & callback,
-    InitFuncT init_func,
-    ParentHandleT parent_handle,
-    EventTypeEnum event_type)
-  : parent_handle_(parent_handle), event_callback_(callback)
-  {
-    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) {
-      if (ret == RCL_RET_UNSUPPORTED) {
-        UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
-        rcl_reset_error();
-        throw exc;
-      } else {
-        rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
-      }
-    }
-  }
-
-  /// Take data so that the callback cannot be scheduled again
-  std::shared_ptr<void>
-  take_data() override
-  {
-    EventCallbackInfoT callback_info;
-    rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
-    if (ret != RCL_RET_OK) {
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Couldn't take event info: %s", rcl_get_error_string().str);
-      return nullptr;
-    }
-    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
-  {
-    if (!data) {
-      throw std::runtime_error("'data' is empty");
-    }
-    auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
-    event_callback_(*callback_ptr);
-    callback_ptr.reset();
-  }
-
-private:
-  using EventCallbackInfoT = typename std::remove_reference<typename
-      rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
-
-  ParentHandleT parent_handle_;
-  EventCallbackT event_callback_;
-};
-
-}  // namespace rclcpp
+#include "rclcpp/event_handler.hpp"
 
 #endif  // RCLCPP__QOS_EVENT_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -117,6 +117,21 @@
  * - Allocator related items:
  *   - rclcpp/allocator/allocator_common.hpp
  *   - rclcpp/allocator/allocator_deleter.hpp
+ * - Dynamic typesupport wrappers
+ *   - rclcpp::dynamic_typesupport::DynamicMessage
+ *   - rclcpp::dynamic_typesupport::DynamicMessageType
+ *   - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder
+ *   - rclcpp::dynamic_typesupport::DynamicSerializationSupport
+ *   - rclcpp/dynamic_typesupport/dynamic_message.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_message_type.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp
+ * - Dynamic typesupport
+ *   - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
+ *   - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
+ * - Dynamic subscription
+ *   - rclcpp::DynamicSubscription
+ *   - rclcpp/dynamic_subscription.hpp
  * - Generic publisher
  *   - rclcpp::Node::create_generic_publisher()
  *   - rclcpp::GenericPublisher
@@ -167,4 +182,6 @@
 #include "rclcpp/waitable.hpp"
 #include "rclcpp/wait_set.hpp"
 
+#include "rclcpp/dynamic_subscription.hpp"
+
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/scope_exit.hpp

@@ -1,56 +0,0 @@
-// Copyright 2015 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.
-
-// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/
-// But I changed the lambda to include by reference rather than value, see:
-// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873
-
-#ifndef RCLCPP__SCOPE_EXIT_HPP_
-#define RCLCPP__SCOPE_EXIT_HPP_
-
-// TODO(christophebedard) remove this header completely in I-turtle
-
-#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead
-
-#include <functional>
-
-#include "rclcpp/macros.hpp"
-
-namespace rclcpp
-{
-
-template<typename Callable>
-struct ScopeExit
-{
-  explicit ScopeExit(Callable callable)
-  : callable_(callable) {}
-  ~ScopeExit() {callable_();}
-
-private:
-  Callable callable_;
-};
-
-template<typename Callable>
-ScopeExit<Callable>
-make_scope_exit(Callable callable)
-{
-  return ScopeExit<Callable>(callable);
-}
-
-}  // namespace rclcpp
-
-#define RCLCPP_SCOPE_EXIT(code) \
-  auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;})
-
-#endif  // RCLCPP__SCOPE_EXIT_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -26,6 +26,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
 #include "rcl/service.h"
+#include "rcl/service_introspection.h"
 
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -34,6 +35,7 @@
 #include "tracetools/tracetools.h"
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -55,7 +57,7 @@ public:
   explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
-  virtual ~ServiceBase();
+  virtual ~ServiceBase() = default;
 
   /// Return the name of the service.
   /** \return The name of the service. */
@@ -222,7 +224,7 @@ public:
     // 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>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -230,7 +232,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_request_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_request_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_request_callback_));
   }
 
@@ -307,11 +310,9 @@ public:
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
     rcl_service_options_t & service_options)
-  : ServiceBase(node_handle), any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
-
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
       new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
@@ -330,7 +331,7 @@ public:
     rcl_ret_t ret = rcl_service_init(
       service_handle_.get(),
       node_handle.get(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &service_options);
     if (ret != RCL_RET_OK) {
@@ -370,8 +371,8 @@ public:
     std::shared_ptr<rcl_node_t> node_handle,
     std::shared_ptr<rcl_service_t> service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle.get())) {
@@ -405,8 +406,8 @@ public:
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle)) {
@@ -486,10 +487,39 @@ public:
     }
   }
 
+  /// Configure client introspection.
+  /**
+   * \param[in] clock clock to use to generate introspection timestamps
+   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_service_configure_service_introspection(
+      service_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection");
+    }
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Service)
 
   AnyServiceCallback<ServiceT> any_callback_;
+
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <vector>
+#include <utility>
 
 #include "rcl/allocator.h"
 
@@ -120,8 +121,8 @@ public:
       }
     }
     for (size_t i = 0; i < waitable_handles_.size(); ++i) {
-      if (!waitable_handles_[i]->is_ready(wait_set)) {
-        waitable_handles_[i].reset();
+      if (waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i]));
       }
     }
 
@@ -145,10 +146,7 @@ public:
       timer_handles_.end()
     );
 
-    waitable_handles_.erase(
-      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
-      waitable_handles_.end()
-    );
+    waitable_handles_.clear();
   }
 
   bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
@@ -392,8 +390,9 @@ public:
     rclcpp::AnyExecutable & any_exec,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
-    auto it = waitable_handles_.begin();
-    while (it != waitable_handles_.end()) {
+    auto & waitable_handles = waitable_triggered_handles_;
+    auto it = waitable_handles.begin();
+    while (it != waitable_handles.end()) {
       std::shared_ptr<Waitable> & waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
@@ -401,7 +400,7 @@ public:
         if (!group) {
           // Group was not found, meaning the waitable is not valid...
           // Remove it from the ready list and continue looking
-          it = waitable_handles_.erase(it);
+          it = waitable_handles.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -414,11 +413,11 @@ public:
         any_exec.waitable = waitable;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
-        waitable_handles_.erase(it);
+        waitable_handles.erase(it);
         return;
       }
       // Else, the waitable is no longer valid, remove it and continue
-      it = waitable_handles_.erase(it);
+      it = waitable_handles.erase(it);
     }
   }
 
@@ -499,6 +498,8 @@ private:
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
   VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
+  VectorRebind<std::shared_ptr<Waitable>> waitable_triggered_handles_;
+
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

rclcpp/include/rclcpp/subscription.hpp

@@ -140,44 +140,15 @@ public:
       node_base,
       type_support_handle,
       topic_name,
-      options.template to_rcl_subscription_options<ROSMessageType>(qos),
-      callback.is_serialized_message_callback()),
+      options.to_rcl_subscription_options(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks,
+      callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE),  // NOLINT
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
   {
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options_.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;
@@ -363,11 +334,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.
@@ -397,6 +388,57 @@ public:
     return any_callback_.use_take_shared_method();
   }
 
+  // DYNAMIC TYPE ==================================================================================
+  // TODO(methylDragon): Reorder later
+  // TODO(methylDragon): Implement later...
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type() override
+  {
+    throw rclcpp::exceptions::UnimplementedError(
+            "get_shared_dynamic_message_type is not implemented for Subscription");
+  }
+
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+  get_shared_dynamic_message() override
+  {
+    throw rclcpp::exceptions::UnimplementedError(
+            "get_shared_dynamic_message is not implemented for Subscription");
+  }
+
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() override
+  {
+    throw rclcpp::exceptions::UnimplementedError(
+            "get_shared_dynamic_serialization_support is not implemented for Subscription");
+  }
+
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+  create_dynamic_message() override
+  {
+    throw rclcpp::exceptions::UnimplementedError(
+            "create_dynamic_message is not implemented for Subscription");
+  }
+
+  void
+  return_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
+  {
+    (void) message;
+    throw rclcpp::exceptions::UnimplementedError(
+            "return_dynamic_message is not implemented for Subscription");
+  }
+
+  void
+  handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) override
+  {
+    (void) message;
+    (void) message_info;
+    throw rclcpp::exceptions::UnimplementedError(
+            "handle_dynamic_message is not implemented for Subscription");
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Subscription)
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -31,13 +31,16 @@
 
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_info.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/serialized_message.hpp"
 #include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
@@ -60,6 +63,14 @@ namespace experimental
 class IntraProcessManager;
 }  // namespace experimental
 
+enum class SubscriptionType : uint8_t
+{
+  ROS_MESSAGE = 1,  // take message as ROS message and handle as ROS message
+  SERIALIZED_MESSAGE = 2,  // take message as serialized and handle as serialized
+  DYNAMIC_MESSAGE_DIRECT = 3,  // take message as DynamicMessage and handle as DynamicMessage
+  DYNAMIC_MESSAGE_FROM_SERIALIZED = 4  // take message as serialized and handle as DynamicMessage
+};
+
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
@@ -76,7 +87,7 @@ public:
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options Options for the subscription.
-   * \param[in] is_serialized is true if the message will be delivered still serialized
+   * \param[in] subscription_type Enum flag to change how the message will be received and delivered
    */
   RCLCPP_PUBLIC
   SubscriptionBase(
@@ -84,12 +95,20 @@ public:
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
-    bool is_serialized = false);
+    const SubscriptionEventCallbacks & event_callbacks,
+    bool use_default_callbacks,
+    SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE);
 
   /// Destructor.
   RCLCPP_PUBLIC
   virtual ~SubscriptionBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(
+    const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this subscription is subscribed on.
   RCLCPP_PUBLIC
   const char *
@@ -107,7 +126,7 @@ public:
   /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
   const
-  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get the actual QoS settings, after the defaults have been determined.
@@ -219,13 +238,13 @@ public:
   const rosidl_message_type_support_t &
   get_message_type_support_handle() const;
 
-  /// Return if the subscription is serialized
+  /// Return the type of the subscription.
   /**
-   * \return `true` if the subscription is serialized, `false` otherwise
+   * \return `SubscriptionType`, which adjusts how messages are received and delivered.
    */
   RCLCPP_PUBLIC
-  bool
-  is_serialized() const;
+  SubscriptionType
+  get_subscription_type() const;
 
   /// Get matching publisher count.
   /** \return The number of publishers on this topic. */
@@ -348,7 +367,7 @@ public:
     // 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>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -356,7 +375,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_message_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_message_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_message_callback_));
   }
 
@@ -448,7 +468,7 @@ public:
    * 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
+   * \sa rclcpp::EventHandlerBase::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
@@ -526,6 +546,49 @@ public:
   rclcpp::ContentFilterOptions
   get_content_filter() const;
 
+  // DYNAMIC TYPE ==================================================================================
+  // TODO(methylDragon): Reorder later
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+  get_shared_dynamic_message() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() = 0;
+
+  /// Borrow a new serialized message (this clones!)
+  /** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+  create_dynamic_message() = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) = 0;
+
+  RCLCPP_PUBLIC
+  bool
+  take_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage & message_out,
+    rclcpp::MessageInfo & message_info_out);
+  // ===============================================================================================
+
 protected:
   template<typename EventCallbackT>
   void
@@ -533,7 +596,7 @@ protected:
     const EventCallbackT & callback,
     const rcl_subscription_event_type_t event_type)
   {
-    auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
+    auto handler = std::make_shared<EventHandler<EventCallbackT,
         std::shared_ptr<rcl_subscription_t>>>(
       callback,
       rcl_subscription_event_init,
@@ -546,6 +609,9 @@ protected:
   RCLCPP_PUBLIC
   void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const;
 
+  RCLCPP_PUBLIC
+  void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
+
   RCLCPP_PUBLIC
   bool
   matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
@@ -556,28 +622,34 @@ protected:
 
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
+  // TODO(methylDragon): Remove if we don't need this
+  // rclcpp::node_interfaces::NodeGraphInterface * const node_graph_;
+  // rclcpp::node_interfaces::NodeServicesInterface * const node_services_;
+
   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::unordered_map<rcl_subscription_event_type_t,
-    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+    std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_subscription_id_;
   std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
 
+  const SubscriptionEventCallbacks event_callbacks_;
+
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
   rosidl_message_type_support_t type_support_;
-  bool is_serialized_;
+  SubscriptionType subscription_type_;
 
   std::atomic<bool> subscription_in_use_by_wait_set_{false};
   std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
-  std::unordered_map<rclcpp::QOSEventHandlerBase *,
+  std::unordered_map<rclcpp::EventHandlerBase *,
     std::atomic<bool>> qos_events_in_use_by_wait_set_;
 
   std::recursive_mutex callback_mutex_;

rclcpp/include/rclcpp/subscription_options.hpp

@@ -26,7 +26,7 @@
 #include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos_overriding_options.hpp"
 #include "rclcpp/subscription_content_filter_options.hpp"
 #include "rclcpp/topic_statistics_state.hpp"
@@ -97,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(
@@ -110,7 +110,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
    * \param qos QoS profile for subcription.
    * \return rcl_subscription_options_t structure based on the rclcpp::QoS
    */
-  template<typename MessageT>
   rcl_subscription_options_t
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {

rclcpp/include/rclcpp/timer.hpp

@@ -149,11 +149,48 @@ public:
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Set a callback to be called when the timer is reset
+  /**
+   * You should aim to make this callback 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.
+   *
+   * Calling it again will override 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,
+   * you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called whenever timer is reset
+   */
+  RCLCPP_PUBLIC
+  void
+  set_on_reset_callback(std::function<void(size_t)> callback);
+
+  /// Unset the callback registered for reset timer
+  RCLCPP_PUBLIC
+  void
+  clear_on_reset_callback();
+
 protected:
+  std::recursive_mutex callback_mutex_;
+  // Declare callback before timer_handle_, so on destruction
+  // the callback is destroyed last. Otherwise, the rcl timer
+  // callback would point briefly to a destroyed function.
+  // Clearing the callback on timer destructor also makes sure
+  // the rcl callback is cleared before on_reset_callback_.
+  std::function<void(size_t)> on_reset_callback_{nullptr};
+
   Clock::SharedPtr clock_;
   std::shared_ptr<rcl_timer_t> timer_handle_;
 
   std::atomic<bool> in_use_by_wait_set_{false};
+
+  RCLCPP_PUBLIC
+  void
+  set_on_reset_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
 
@@ -189,11 +226,17 @@ public:
     TRACEPOINT(
       rclcpp_timer_callback_added,
       static_cast<const void *>(get_timer_handle().get()),
-      static_cast<const void *>(&callback_));
-    TRACEPOINT(
-      rclcpp_callback_register,
-      static_cast<const void *>(&callback_),
-      tracetools::get_symbol(callback_));
+      reinterpret_cast<const void *>(&callback_));
+#ifndef TRACETOOLS_DISABLED
+    if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+      char * symbol = tracetools::get_symbol(callback_);
+      DO_TRACEPOINT(
+        rclcpp_callback_register,
+        reinterpret_cast<const void *>(&callback_),
+        symbol);
+      std::free(symbol);
+    }
+#endif
   }
 
   /// Default destructor.
@@ -226,9 +269,9 @@ public:
   void
   execute_callback() override
   {
-    TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
+    TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
-    TRACEPOINT(callback_end, static_cast<const void *>(&callback_));
+    TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
   }
 
   // void specialization

rclcpp/include/rclcpp/utilities.hpp

@@ -169,7 +169,7 @@ remove_ros_arguments(int argc, char const * const * argv);
  * the context initialized by rclcpp::init().
  *
  * \param[in] context Optional check for shutdown of this Context.
- * \return true if shutdown has been called, false otherwise
+ * \return false if shutdown has been called, true otherwise
  */
 RCLCPP_PUBLIC
 bool

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -516,7 +516,7 @@ public:
    * the waitable to be removed, but it will cause the associated entity pointer
    * to be nullptr when introspecting this waitable after waiting.
    *
-   * Note that rclcpp::QOSEventHandlerBase are just a special case of
+   * Note that rclcpp::EventHandlerBase is just a special case of
    * rclcpp::Waitable and can be added with this function.
    *
    * \param[in] waitable Waitable to be added.

rclcpp/package.xml

@@ -2,13 +2,17 @@
 <?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>16.0.1</version>
+  <version>19.3.0</version>
   <description>The ROS client library in C++.</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
@@ -35,6 +39,7 @@
   <depend>rcpputils</depend>
   <depend>rcutils</depend>
   <depend>rmw</depend>
+  <depend>rosidl_dynamic_typesupport</depend>
   <depend>statistics_msgs</depend>
   <depend>tracetools</depend>
 

rclcpp/resource/logging.hpp.em

@@ -114,7 +114,6 @@ def get_rclcpp_suffix_from_features(features):
 @[ end for]@
 @[ if 'stream' not in feature_combination]@
  * \param ... The format string, followed by the variable arguments for the format string.
- * It also accepts a single argument of type std::string.
 @[ end if]@
  */
 @{params = rclcpp_feature_combinations[feature_combination].params.keys()}@
@@ -125,7 +124,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, \
+      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \

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()
@@ -97,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
+rclcpp::GuardCondition::SharedPtr
+CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
+    if (associated_with_executor_) {
+      trigger_notify_guard_condition();
+    }
+    notify_guard_condition_ = nullptr;
+  }
+
+  if (!notify_guard_condition_) {
+    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
+  }
+
+  return notify_guard_condition_;
+}
+
+void
+CallbackGroup::trigger_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_) {
+    notify_guard_condition_->trigger();
+  }
+}
+
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/client.cpp

@@ -23,9 +23,11 @@
 #include "rcl/graph.h"
 #include "rcl/node.h"
 #include "rcl/wait.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/logging.hpp"
 
@@ -65,13 +67,6 @@ 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();
-}
-
 bool
 ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out)
 {
@@ -248,7 +243,6 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo
     user_data);
 
   if (RCL_RET_OK != ret) {
-    using rclcpp::exceptions::throw_from_rcl_error;
     throw_from_rcl_error(ret, "failed to set the on new response callback for client");
   }
 }

rclcpp/src/rclcpp/clock.cpp

@@ -67,7 +67,7 @@ Clock::Clock(rcl_clock_type_t clock_type)
 Clock::~Clock() {}
 
 Time
-Clock::now()
+Clock::now() const
 {
   Time now(0, 0, impl_->rcl_clock_.type);
 
@@ -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()
 {

rclcpp/src/rclcpp/context.cpp

@@ -154,7 +154,9 @@ Context::~Context()
   // this will not prevent errors, but will maybe make them easier to reproduce
   std::lock_guard<std::recursive_mutex> lock(init_mutex_);
   try {
-    this->shutdown("context destructor was called while still not shutdown");
+    // Cannot rely on virtual dispatch in a destructor, so explicitly use the
+    // shutdown() provided by this base class.
+    Context::shutdown("context destructor was called while still not shutdown");
     // at this point it is shutdown and cannot reinit
     // clean_up will finalize the rcl context
     this->clean_up();
@@ -217,7 +219,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();
@@ -363,49 +365,45 @@ Context::on_shutdown(OnShutdownCallback callback)
 rclcpp::OnShutdownCallbackHandle
 Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  return add_shutdown_callback(ShutdownType::on_shutdown, callback);
+  return add_shutdown_callback<ShutdownType::on_shutdown>(callback);
 }
 
 bool
 Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle);
+  return remove_shutdown_callback<ShutdownType::on_shutdown>(callback_handle);
 }
 
 rclcpp::PreShutdownCallbackHandle
 Context::add_pre_shutdown_callback(PreShutdownCallback callback)
 {
-  return add_shutdown_callback(ShutdownType::pre_shutdown, callback);
+  return add_shutdown_callback<ShutdownType::pre_shutdown>(callback);
 }
 
 bool
 Context::remove_pre_shutdown_callback(
   const PreShutdownCallbackHandle & callback_handle)
 {
-  return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
+  return remove_shutdown_callback<ShutdownType::pre_shutdown>(callback_handle);
 }
 
+template<Context::ShutdownType shutdown_type>
 rclcpp::ShutdownCallbackHandle
 Context::add_shutdown_callback(
-  ShutdownType shutdown_type,
   ShutdownCallback callback)
 {
   auto callback_shared_ptr =
     std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);
 
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      {
-        std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
-        pre_shutdown_callbacks_.emplace(callback_shared_ptr);
-      }
-      break;
-    case ShutdownType::on_shutdown:
-      {
-        std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-        on_shutdown_callbacks_.emplace(callback_shared_ptr);
-      }
-      break;
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
+    pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);
+  } else {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    on_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   }
 
   ShutdownCallbackHandle callback_handle;
@@ -413,73 +411,74 @@ Context::add_shutdown_callback(
   return callback_handle;
 }
 
+template<Context::ShutdownType shutdown_type>
 bool
 Context::remove_shutdown_callback(
-  ShutdownType shutdown_type,
   const ShutdownCallbackHandle & callback_handle)
 {
-  std::mutex * mutex_ptr = nullptr;
-  std::unordered_set<
-    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
-
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      mutex_ptr = &pre_shutdown_callbacks_mutex_;
-      callback_list_ptr = &pre_shutdown_callbacks_;
-      break;
-    case ShutdownType::on_shutdown:
-      mutex_ptr = &on_shutdown_callbacks_mutex_;
-      callback_list_ptr = &on_shutdown_callbacks_;
-      break;
-  }
-
-  std::lock_guard<std::mutex> lock(*mutex_ptr);
-  auto callback_shared_ptr = callback_handle.callback.lock();
+  const auto callback_shared_ptr = callback_handle.callback.lock();
   if (callback_shared_ptr == nullptr) {
     return false;
   }
-  return callback_list_ptr->erase(callback_shared_ptr) == 1;
+
+  const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {
+      const std::lock_guard<std::mutex> lock(mutex);
+      auto iter = callback_vector.begin();
+      for (; iter != callback_vector.end(); iter++) {
+        if ((*iter).get() == callback_shared_ptr.get()) {
+          break;
+        }
+      }
+      if (iter == callback_vector.end()) {
+        return false;
+      }
+      callback_vector.erase(iter);
+      return true;
+    };
+
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+  } else {
+    return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
+  }
 }
 
 std::vector<rclcpp::Context::OnShutdownCallback>
 Context::get_on_shutdown_callbacks() const
 {
-  return get_shutdown_callback(ShutdownType::on_shutdown);
+  return get_shutdown_callback<ShutdownType::on_shutdown>();
 }
 
 std::vector<rclcpp::Context::PreShutdownCallback>
 Context::get_pre_shutdown_callbacks() const
 {
-  return get_shutdown_callback(ShutdownType::pre_shutdown);
+  return get_shutdown_callback<ShutdownType::pre_shutdown>();
 }
 
+template<Context::ShutdownType shutdown_type>
 std::vector<rclcpp::Context::ShutdownCallback>
-Context::get_shutdown_callback(ShutdownType shutdown_type) const
+Context::get_shutdown_callback() const
 {
-  std::mutex * mutex_ptr = nullptr;
-  const std::unordered_set<
-    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+  const auto get_callback_vector = [this](auto & mutex, auto & callback_set) {
+      const std::lock_guard<std::mutex> lock(mutex);
+      std::vector<rclcpp::Context::ShutdownCallback> callbacks;
+      for (auto & callback : callback_set) {
+        callbacks.push_back(*callback);
+      }
+      return callbacks;
+    };
 
-  switch (shutdown_type) {
-    case ShutdownType::pre_shutdown:
-      mutex_ptr = &pre_shutdown_callbacks_mutex_;
-      callback_list_ptr = &pre_shutdown_callbacks_;
-      break;
-    case ShutdownType::on_shutdown:
-      mutex_ptr = &on_shutdown_callbacks_mutex_;
-      callback_list_ptr = &on_shutdown_callbacks_;
-      break;
+  static_assert(
+    shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
+
+  if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
+    return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+  } else {
+    return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
   }
-
-  std::vector<rclcpp::Context::ShutdownCallback> callbacks;
-  {
-    std::lock_guard<std::mutex> lock(*mutex_ptr);
-    for (auto & iter : *callback_list_ptr) {
-      callbacks.emplace_back(*iter);
-    }
-  }
-
-  return callbacks;
 }
 
 std::shared_ptr<rcl_context_t>

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp

@@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides(
         [params]() {
           rcl_yaml_node_struct_fini(params);
         });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());
 
-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            result[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
+      if (initial_map.count(node_fqn) > 0) {
+        // Combine parameter yaml files, overwriting values in older ones
+        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
+          result[param.get_name()] =
+            rclcpp::ParameterValue(param.get_value_message());
         }
       }
     }

rclcpp/src/rclcpp/duration.cpp

@@ -161,6 +161,13 @@ Duration::operator+(const rclcpp::Duration & rhs) const
     rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
 }
 
+Duration &
+Duration::operator+=(const rclcpp::Duration & rhs)
+{
+  *this = *this + rhs;
+  return *this;
+}
+
 void
 bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max)
 {
@@ -190,6 +197,13 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }
 
+Duration &
+Duration::operator-=(const rclcpp::Duration & rhs)
+{
+  *this = *this - rhs;
+  return *this;
+}
+
 void
 bounds_check_duration_scale(int64_t dns, double scale, uint64_t max)
 {
@@ -222,6 +236,13 @@ Duration::operator*(double scale) const
       static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
 
+Duration &
+Duration::operator*=(double scale)
+{
+  *this = *this * scale;
+  return *this;
+}
+
 rcl_duration_value_t
 Duration::nanoseconds() const
 {

rclcpp/src/rclcpp/dynamic_subscription.cpp

@@ -0,0 +1,113 @@
+// 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.
+
+#include "rclcpp/dynamic_subscription.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rcl/subscription.h"
+
+#include "rclcpp/exceptions.hpp"
+
+namespace rclcpp
+{
+
+std::shared_ptr<void> DynamicSubscription::create_message()
+{
+  return create_serialized_message();
+}
+
+std::shared_ptr<rclcpp::SerializedMessage> DynamicSubscription::create_serialized_message()
+{
+  return std::make_shared<rclcpp::SerializedMessage>(0);
+}
+
+void DynamicSubscription::handle_message(std::shared_ptr<void> &, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::handle_serialized_message(
+  const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_serialized_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::handle_loaned_message(void *, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_loaned_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::return_message(std::shared_ptr<void> & message)
+{
+  auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
+  return_serialized_message(typed_message);
+}
+
+void DynamicSubscription::return_serialized_message(
+  std::shared_ptr<rclcpp::SerializedMessage> & message)
+{
+  message.reset();
+}
+
+
+// DYNAMIC TYPE ====================================================================================
+// TODO(methylDragon): Re-order later
+
+// Does not clone
+rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+DynamicSubscription::get_shared_dynamic_message_type()
+{
+  return dynamic_message_type_;
+}
+
+// Does not clone
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+DynamicSubscription::get_shared_dynamic_message()
+{
+  return dynamic_message_;
+}
+
+rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+DynamicSubscription::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+DynamicSubscription::create_dynamic_message()
+{
+  return dynamic_message_->init_from_type_shared(*dynamic_message_type_);
+}
+
+void
+DynamicSubscription::return_dynamic_message(
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
+{
+  message.reset();
+}
+
+void DynamicSubscription::handle_dynamic_message(
+  const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+  const rclcpp::MessageInfo & message_info)
+{
+  (void) message_info;
+  callback_(message, ts_->get_shared_rosidl_runtime_c_type_description());
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp

@@ -0,0 +1,838 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rcl/types.h"
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+
+
+using rclcpp::dynamic_typesupport::DynamicMessage;
+using rclcpp::dynamic_typesupport::DynamicMessageType;
+using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp"
+#endif
+
+
+// CONSTRUCTION ==================================================================================
+DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
+: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!serialization_support_) {
+    throw std::runtime_error("dynamic type could not bind serialization support!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder =
+    dynamic_type_builder->get_rosidl_dynamic_type_builder();
+  if (!rosidl_dynamic_type_builder) {
+    throw std::runtime_error("dynamic type builder cannot be nullptr!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type_builder(
+    rosidl_dynamic_type_builder, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error("could not create new dynamic data object from dynamic type builder");
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_dynamic_data,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type)
+: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!serialization_support_) {
+    throw std::runtime_error("dynamic type could not bind serialization support!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type =
+    dynamic_type->get_rosidl_dynamic_type();
+  if (!rosidl_dynamic_type) {
+    throw std::runtime_error("dynamic type cannot be nullptr!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
+    rosidl_dynamic_type, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error(
+            std::string("could not create new dynamic data object from dynamic type") +
+            rcl_get_error_string().str);
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_dynamic_data,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicMessage::DynamicMessage(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!rosidl_dynamic_data) {
+    throw std::runtime_error("rosidl dynamic data cannot be nullptr!");
+  }
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic data's!");
+    }
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_dynamic_data,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicMessage::DynamicMessage(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_data_(rosidl_dynamic_data),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!rosidl_dynamic_data) {
+    throw std::runtime_error("rosidl dynamic data cannot be nullptr!");
+  }
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic data's!");
+    }
+  }
+}
+
+
+DynamicMessage::DynamicMessage(
+  DynamicMessage::SharedPtr parent_data,
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data)
+: serialization_support_(parent_data->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(true),
+  parent_data_(nullptr)
+{
+  if (!parent_data) {
+    throw std::runtime_error("parent dynamic data cannot be nullptr!");
+  }
+  if (!rosidl_loaned_data) {
+    throw std::runtime_error("loaned rosidl dynamic data cannot be nullptr!");
+  }
+
+  if (serialization_support_) {
+    if (!match_serialization_support_(*serialization_support_, *rosidl_loaned_data)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match loaned dynamic data's!");
+    }
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_loaned_data,
+    // Custom no-op deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      // Data fini and destruction is deferred to return_loaned_value()
+      (void) rosidl_dynamic_data;
+    });
+  parent_data_ = parent_data;
+}
+
+
+DynamicMessage::DynamicMessage(const DynamicMessage & other)
+: enable_shared_from_this(),
+  serialization_support_(nullptr),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  DynamicMessage out = other.clone();
+  // We don't copy is_loaned_ or parent_data_ because it's a fresh copy now
+  std::swap(serialization_support_, out.serialization_support_);
+  std::swap(rosidl_dynamic_data_, out.rosidl_dynamic_data_);
+}
+
+
+DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_data_(std::exchange(other.rosidl_dynamic_data_, nullptr)),
+  is_loaned_(other.is_loaned_),
+  parent_data_(std::exchange(other.parent_data_, nullptr))
+{}
+
+
+DynamicMessage &
+DynamicMessage::operator=(const DynamicMessage & other)
+{
+  return *this = DynamicMessage(other);
+}
+
+
+DynamicMessage &
+DynamicMessage::operator=(DynamicMessage && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_);
+  is_loaned_ = other.is_loaned_;
+  std::swap(parent_data_, other.parent_data_);
+  return *this;
+}
+
+
+DynamicMessage::~DynamicMessage()
+{
+  if (is_loaned_) {
+    if (!parent_data_) {
+      RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!");
+    } else {
+      rosidl_dynamic_typesupport_dynamic_data_return_loaned_value(
+        parent_data_->get_rosidl_dynamic_data(), get_rosidl_dynamic_data());
+    }
+  }
+}
+
+
+bool
+DynamicMessage::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data)
+{
+  bool out = true;
+
+  if (serialization_support.get_library_identifier() != std::string(
+      rosidl_dynamic_type_data.serialization_support->library_identifier))
+  {
+    RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's");
+    out = false;
+  }
+
+  // TODO(methylDragon): Can I do this?? Is it portable?
+  if (serialization_support.get_rosidl_serialization_support() !=
+    rosidl_dynamic_type_data.serialization_support)
+  {
+    RCUTILS_LOG_ERROR("serialization support pointer does not match dynamic data's");
+    out = false;
+  }
+
+  return out;
+}
+
+
+// GETTERS =======================================================================================
+const std::string
+DynamicMessage::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_data_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicMessage::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  if (
+    rosidl_dynamic_typesupport_dynamic_data_get_name(
+      get_rosidl_dynamic_data(), &buf,
+      &buf_length) !=
+    RCUTILS_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("could not get name for dynamic data") + rcl_get_error_string().str);
+  }
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_data_t *
+DynamicMessage::get_rosidl_dynamic_data()
+{
+  return rosidl_dynamic_data_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_data_t *
+DynamicMessage::get_rosidl_dynamic_data() const
+{
+  return rosidl_dynamic_data_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
+DynamicMessage::get_shared_rosidl_dynamic_data()
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>(
+    shared_from_this(), rosidl_dynamic_data_.get());
+}
+
+
+std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_data_t>
+DynamicMessage::get_shared_rosidl_dynamic_data() const
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>(
+    shared_from_this(), rosidl_dynamic_data_.get());
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessage::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessage::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+size_t
+DynamicMessage::get_item_count() const
+{
+  size_t item_count;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count(
+    get_rosidl_dynamic_data(), &item_count);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not get item count of dynamic data");
+  }
+  return item_count;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_member_id(size_t index) const
+{
+  rosidl_dynamic_typesupport_member_id_t member_id;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index(
+    get_rosidl_dynamic_data(), index, &member_id);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not member id of dynamic data element by index");
+  }
+  return member_id;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_member_id(const std::string & name) const
+{
+  rosidl_dynamic_typesupport_member_id_t member_id;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name(
+    get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not member id of dynamic data element by name");
+  }
+  return member_id;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_array_index(size_t index) const
+{
+  rosidl_dynamic_typesupport_member_id_t array_index;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index(
+    get_rosidl_dynamic_data(), index, &array_index);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not array index of dynamic data element by index");
+  }
+  return array_index;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_array_index(const std::string & name) const
+{
+  return get_array_index(get_member_id(name));
+}
+
+
+// METHODS =======================================================================================
+DynamicMessage
+DynamicMessage::clone() const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
+    get_rosidl_dynamic_data(), &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage(serialization_support_, rosidl_dynamic_data);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::clone_shared() const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
+    get_rosidl_dynamic_data(), &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data);
+}
+
+
+DynamicMessage
+DynamicMessage::init_from_type(DynamicMessageType & type) const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
+    type.get_rosidl_dynamic_type(), &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error("could not create new dynamic data object from dynamic type");
+  }
+  return DynamicMessage(serialization_support_, rosidl_dynamic_data);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::init_from_type_shared(DynamicMessageType & type) const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
+    type.get_rosidl_dynamic_type(), &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error("could not create new dynamic data object from dynamic type");
+  }
+  return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data);
+}
+
+
+bool
+DynamicMessage::equals(const DynamicMessage & other) const
+{
+  if (get_library_identifier() != other.get_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  bool equals;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals(
+    get_rosidl_dynamic_data(), other.get_rosidl_dynamic_data(), &equals);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not equate dynamic messages");
+  }
+  return equals;
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::loan_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value(
+    get_rosidl_dynamic_data(), id, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
+    throw std::runtime_error(
+            std::string("could not loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage::make_shared(shared_from_this(), rosidl_dynamic_data);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::loan_value(const std::string & name)
+{
+  return loan_value(get_member_id(name));
+}
+
+
+void
+DynamicMessage::clear_all_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_all_values(get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::clear_nonkey_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_value(get_rosidl_dynamic_data(), id);
+}
+
+
+void
+DynamicMessage::clear_value(const std::string & name)
+{
+  clear_value(get_member_id(name));
+}
+
+
+void
+DynamicMessage::clear_sequence()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(get_rosidl_dynamic_data());
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_sequence_data()
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(get_rosidl_dynamic_data(), &out);
+  return out;
+}
+
+
+void
+DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index)
+{
+  rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data(
+    get_rosidl_dynamic_data(), index);
+}
+
+
+bool
+DynamicMessage::serialize(rcl_serialized_message_t * buffer)
+{
+  bool success;
+  rcutils_ret_t ret =
+    rosidl_dynamic_typesupport_dynamic_data_serialize(get_rosidl_dynamic_data(), buffer, &success);
+  if (ret != RCUTILS_RET_OK || !success) {
+    throw std::runtime_error(
+            std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return success;
+}
+
+
+bool
+DynamicMessage::deserialize(rcl_serialized_message_t * buffer)
+{
+  bool success;
+  rcutils_ret_t ret =
+    rosidl_dynamic_typesupport_dynamic_data_deserialize(
+    get_rosidl_dynamic_data(), buffer,
+    &success);
+  if (ret != RCUTILS_RET_OK || !success) {
+    throw std::runtime_error(
+            std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return success;
+}
+
+
+// MEMBER ACCESS ===================================================================================
+// Defined in "detail/dynamic_message_impl.hpp"
+
+
+// FIXED STRING MEMBER ACCESS ======================================================================
+const std::string
+DynamicMessage::get_fixed_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t string_length)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::string
+DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length)
+{
+  return get_fixed_string_value(get_member_id(name), string_length);
+}
+
+
+const std::u16string
+DynamicMessage::get_fixed_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::u16string
+DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length)
+{
+  return get_fixed_wstring_value(get_member_id(name), wstring_length);
+}
+
+
+void
+DynamicMessage::set_fixed_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value(
+    get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length);
+}
+
+
+void
+DynamicMessage::set_fixed_string_value(
+  const std::string & name, const std::string value, size_t string_length)
+{
+  set_fixed_string_value(get_member_id(name), value, string_length);
+}
+
+
+void
+DynamicMessage::set_fixed_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value(
+    get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length);
+}
+
+
+void
+DynamicMessage::set_fixed_wstring_value(
+  const std::string & name, const std::u16string value, size_t wstring_length)
+{
+  set_fixed_wstring_value(get_member_id(name), value, wstring_length);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value(
+    get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value(
+    get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out);
+  return out;
+}
+
+
+// BOUNDED STRING MEMBER ACCESS ====================================================================
+const std::string
+DynamicMessage::get_bounded_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t string_bound)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::string
+DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound)
+{
+  return get_bounded_string_value(get_member_id(name), string_bound);
+}
+
+
+const std::u16string
+DynamicMessage::get_bounded_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value(
+    get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::u16string
+DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound)
+{
+  return get_bounded_wstring_value(get_member_id(name), wstring_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value(
+    get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_string_value(
+  const std::string & name, const std::string value, size_t string_bound)
+{
+  set_bounded_string_value(get_member_id(name), value, string_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value(
+    get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_wstring_value(
+  const std::string & name, const std::u16string value, size_t wstring_bound)
+{
+  set_bounded_wstring_value(get_member_id(name), value, wstring_bound);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value(
+    get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value(
+    get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out);
+  return out;
+}
+
+
+// NESTED MEMBER ACCESS ============================================================================
+DynamicMessage
+DynamicMessage::get_complex_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_complex_value(
+    get_rosidl_dynamic_data(), id, &out_ptr);
+  return DynamicMessage(get_shared_dynamic_serialization_support(), out_ptr);
+}
+
+
+DynamicMessage
+DynamicMessage::get_complex_value(const std::string & name)
+{
+  return get_complex_value(get_member_id(name));
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_complex_value(
+    get_rosidl_dynamic_data(), id, &out_ptr);
+  return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), out_ptr);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::get_complex_value_shared(const std::string & name)
+{
+  return get_complex_value_shared(get_member_id(name));
+}
+
+
+void
+DynamicMessage::set_complex_value(
+  rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_complex_value(
+    get_rosidl_dynamic_data(), id, value.get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::set_complex_value(const std::string & name, DynamicMessage & value)
+{
+  set_complex_value(get_member_id(name), value);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_complex_value_copy(const DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_complex_value_copy(
+    get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_complex_value(DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_complex_value(
+    get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out);
+  return out;
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp

@@ -0,0 +1,338 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+
+
+using rclcpp::dynamic_typesupport::DynamicMessage;
+using rclcpp::dynamic_typesupport::DynamicMessageType;
+using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
+: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_type_(nullptr)
+{
+  if (!serialization_support_) {
+    throw std::runtime_error("dynamic type could not bind serialization support!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder =
+    dynamic_type_builder->get_rosidl_dynamic_type_builder();
+  if (!rosidl_dynamic_type_builder) {
+    throw std::runtime_error("dynamic type builder cannot be nullptr!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_dynamic_type_builder(
+    rosidl_dynamic_type_builder, &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
+    throw std::runtime_error("could not create new dynamic type object");
+  }
+
+  rosidl_dynamic_type_.reset(
+    rosidl_dynamic_type,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
+      rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
+    });
+}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)
+: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr)
+{
+  if (!rosidl_dynamic_type) {
+    throw std::runtime_error("rosidl dynamic type cannot be nullptr!");
+  }
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic type's!");
+    }
+  }
+
+  rosidl_dynamic_type_.reset(
+    rosidl_dynamic_type,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
+      rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
+    });
+}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type)
+: serialization_support_(serialization_support), rosidl_dynamic_type_(rosidl_dynamic_type)
+{
+  if (!rosidl_dynamic_type) {
+    throw std::runtime_error("rosidl dynamic type cannot be nullptr!");
+  }
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic type's!");
+    }
+  }
+}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description)
+: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr)
+{
+  init_from_description(description, serialization_support);
+}
+
+
+DynamicMessageType::DynamicMessageType(const DynamicMessageType & other)
+: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_(nullptr)
+{
+  DynamicMessageType out = other.clone();
+  std::swap(serialization_support_, out.serialization_support_);
+  std::swap(rosidl_dynamic_type_, out.rosidl_dynamic_type_);
+}
+
+
+DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_(std::exchange(other.rosidl_dynamic_type_, nullptr)) {}
+
+
+DynamicMessageType &
+DynamicMessageType::operator=(const DynamicMessageType & other)
+{
+  return *this = DynamicMessageType(other);
+}
+
+
+DynamicMessageType &
+DynamicMessageType::operator=(DynamicMessageType && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_);
+  return *this;
+}
+
+
+DynamicMessageType::~DynamicMessageType() {}
+
+
+void
+DynamicMessageType::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (serialization_support) {
+    // Swap serialization support if serialization support is given
+    serialization_support_ = serialization_support;
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_description(
+    serialization_support_->get_rosidl_serialization_support(), &description, &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
+    throw std::runtime_error("could not create new dynamic type object");
+  }
+
+  rosidl_dynamic_type_.reset(
+    rosidl_dynamic_type,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
+      rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
+    });
+}
+
+
+bool
+DynamicMessageType::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type)
+{
+  bool out = true;
+
+  if (serialization_support.get_library_identifier() != std::string(
+      rosidl_dynamic_type.serialization_support->library_identifier))
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support library identifier does not match dynamic type's");
+    out = false;
+  }
+
+  // TODO(methylDragon): Can I do this?? Is it portable?
+  if (serialization_support.get_rosidl_serialization_support() !=
+    rosidl_dynamic_type.serialization_support)
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support pointer does not match dynamic type's");
+    out = false;
+  }
+
+  return out;
+}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicMessageType::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_type_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicMessageType::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  rosidl_dynamic_typesupport_dynamic_type_get_name(get_rosidl_dynamic_type(), &buf, &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+size_t
+DynamicMessageType::get_member_count() const
+{
+  size_t out;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count(
+    rosidl_dynamic_type_.get(), &out);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not get member count: ") + rcl_get_error_string().str);
+  }
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_t *
+DynamicMessageType::get_rosidl_dynamic_type()
+{
+  return rosidl_dynamic_type_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_t *
+DynamicMessageType::get_rosidl_dynamic_type() const
+{
+  return rosidl_dynamic_type_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
+DynamicMessageType::get_shared_rosidl_dynamic_type()
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>(
+    shared_from_this(), rosidl_dynamic_type_.get());
+}
+
+
+std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_t>
+DynamicMessageType::get_shared_rosidl_dynamic_type() const
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>(
+    shared_from_this(), rosidl_dynamic_type_.get());
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageType::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageType::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =========================================================================================
+DynamicMessageType
+DynamicMessageType::clone() const
+{
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
+    get_rosidl_dynamic_type(), &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageType(serialization_support_, rosidl_dynamic_type);
+}
+
+
+DynamicMessageType::SharedPtr
+DynamicMessageType::clone_shared() const
+{
+  rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
+    get_rosidl_dynamic_type(), &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageType::make_shared(serialization_support_, rosidl_dynamic_type);
+}
+
+
+bool
+DynamicMessageType::equals(const DynamicMessageType & other) const
+{
+  if (get_library_identifier() != other.get_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  bool out;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals(
+    get_rosidl_dynamic_type(), other.get_rosidl_dynamic_type(), &out);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not equate dynamic message types: ") + rcl_get_error_string().str);
+  }
+  return out;
+}
+
+
+DynamicMessage
+DynamicMessageType::build_dynamic_message()
+{
+  return DynamicMessage(shared_from_this());
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessageType::build_dynamic_message_shared()
+{
+  return DynamicMessage::make_shared(shared_from_this());
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp

@@ -0,0 +1,675 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+#include <memory>
+#include <string>
+
+#include "rcutils/logging_macros.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+
+
+using rclcpp::dynamic_typesupport::DynamicMessage;
+using rclcpp::dynamic_typesupport::DynamicMessageType;
+using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp"
+#endif
+
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name)
+: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr)
+{
+  init_from_serialization_support_(serialization_support, name);
+  if (!rosidl_dynamic_type_builder_) {
+    throw std::runtime_error("could not create new dynamic type builder object");
+  }
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)
+: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr)
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+  if (!rosidl_dynamic_type_builder) {
+    throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!");
+  }
+  if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder)) {
+    throw std::runtime_error(
+            "serialization support library does not match dynamic type builder's!");
+  }
+
+  rosidl_dynamic_type_builder_.reset(
+    rosidl_dynamic_type_builder,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
+      rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
+    });
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> rosidl_dynamic_type_builder)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_builder_(rosidl_dynamic_type_builder)
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+  if (!rosidl_dynamic_type_builder) {
+    throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!");
+  }
+  if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder.get())) {
+    throw std::runtime_error(
+            "serialization support library does not match dynamic type builder's!");
+  }
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_builder_(nullptr)
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+  init_from_description(description, serialization_support);
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other)
+: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_builder_(nullptr)
+{
+  DynamicMessageTypeBuilder out = other.clone();
+  std::swap(serialization_support_, out.serialization_support_);
+  std::swap(rosidl_dynamic_type_builder_, out.rosidl_dynamic_type_builder_);
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_builder_(std::exchange(other.rosidl_dynamic_type_builder_, nullptr)) {}
+
+
+DynamicMessageTypeBuilder &
+DynamicMessageTypeBuilder::operator=(const DynamicMessageTypeBuilder & other)
+{
+  return *this = DynamicMessageTypeBuilder(other);
+}
+
+
+DynamicMessageTypeBuilder &
+DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_);
+  return *this;
+}
+
+
+DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() {}
+
+
+void
+DynamicMessageTypeBuilder::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (serialization_support) {
+    // Swap serialization support if serialization support is given
+    serialization_support_ = serialization_support;
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create_from_description(
+    serialization_support_->get_rosidl_serialization_support(), &description,
+    &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
+    throw std::runtime_error("could not create new dynamic type builder object");
+  }
+
+  rosidl_dynamic_type_builder_.reset(
+    rosidl_dynamic_type_builder,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
+      rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
+    });
+}
+
+
+void
+DynamicMessageTypeBuilder::init_from_serialization_support_(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const std::string & name)
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+  if (!serialization_support->get_rosidl_serialization_support()) {
+    throw std::runtime_error("serialization support raw pointer cannot be nullptr!");
+  }
+
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create(
+    serialization_support->get_rosidl_serialization_support(),
+    name.c_str(), name.size(),
+    &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not init dynamic type builder: ") + rcl_get_error_string().str);
+  }
+  if (!rosidl_dynamic_type_builder) {
+    throw std::runtime_error("could not init dynamic type builder object");
+  }
+
+  rosidl_dynamic_type_builder_.reset(
+    rosidl_dynamic_type_builder,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
+      rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
+    });
+}
+
+
+bool
+DynamicMessageTypeBuilder::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder)
+{
+  bool out = true;
+
+  if (serialization_support.get_library_identifier() != std::string(
+      rosidl_dynamic_type_builder.serialization_support->library_identifier))
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support library identifier does not match dynamic type builder's");
+    out = false;
+  }
+
+  // TODO(methylDragon): Can I do this?? Is it portable?
+  if (serialization_support.get_rosidl_serialization_support() !=
+    rosidl_dynamic_type_builder.serialization_support)
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support pointer does not match dynamic type builder's");
+    out = false;
+  }
+
+  return out;
+}
+
+
+// GETTERS =======================================================================================
+const std::string
+DynamicMessageTypeBuilder::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_type_builder_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicMessageTypeBuilder::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  rosidl_dynamic_typesupport_dynamic_type_builder_get_name(
+    get_rosidl_dynamic_type_builder(), &buf, &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_builder_t *
+DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder()
+{
+  return rosidl_dynamic_type_builder_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_builder_t *
+DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const
+{
+  return rosidl_dynamic_type_builder_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>
+DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder()
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>(
+    shared_from_this(), rosidl_dynamic_type_builder_.get());
+}
+
+
+std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_builder_t>
+DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder() const
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>(
+    shared_from_this(), rosidl_dynamic_type_builder_.get());
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =======================================================================================
+void
+DynamicMessageTypeBuilder::set_name(const std::string & name)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_set_name(
+    get_rosidl_dynamic_type_builder(), name.c_str(), name.size());
+}
+
+
+DynamicMessageTypeBuilder
+DynamicMessageTypeBuilder::clone() const
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
+    get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageTypeBuilder(serialization_support_, rosidl_dynamic_type_builder);
+}
+
+
+DynamicMessageTypeBuilder::SharedPtr
+DynamicMessageTypeBuilder::clone_shared() const
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
+    get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageTypeBuilder::make_shared(
+    serialization_support_, rosidl_dynamic_type_builder);
+}
+
+
+void
+DynamicMessageTypeBuilder::clear()
+{
+  if (!serialization_support_) {
+    throw std::runtime_error(
+            "cannot call clear() on a dynamic type builder with uninitialized serialization support"
+    );
+  }
+
+  const std::string & name = get_name();
+  init_from_serialization_support_(serialization_support_, name);
+  if (!rosidl_dynamic_type_builder_) {
+    throw std::runtime_error("could not create new dynamic type builder object");
+  }
+}
+
+
+DynamicMessage
+DynamicMessageTypeBuilder::build_dynamic_message()
+{
+  return DynamicMessage(shared_from_this());
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessageTypeBuilder::build_dynamic_message_shared()
+{
+  return DynamicMessage::make_shared(shared_from_this());
+}
+
+
+DynamicMessageType
+DynamicMessageTypeBuilder::build_dynamic_message_type()
+{
+  return DynamicMessageType(shared_from_this());
+}
+
+
+DynamicMessageType::SharedPtr
+DynamicMessageTypeBuilder::build_dynamic_message_type_shared()
+{
+  return DynamicMessageType::make_shared(shared_from_this());
+}
+
+
+// ADD MEMBERS =====================================================================================
+// Defined in "detail/dynamic_message_type_builder_impl.hpp"
+
+
+// ADD FIXED STRING MEMBERS ========================================================================
+void
+DynamicMessageTypeBuilder::add_fixed_string_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_length, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_length, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_length, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length, sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_length, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length, sequence_bound);
+}
+
+
+// ADD BOUNDED STRING MEMBERS ======================================================================
+void
+DynamicMessageTypeBuilder::add_bounded_string_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_bound, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_bound, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_bound, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound, sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_bound, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound, sequence_bound);
+}
+
+
+// ADD NESTED MEMBERS ==============================================================================
+void
+DynamicMessageTypeBuilder::add_complex_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type.get_rosidl_dynamic_type(), array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type.get_rosidl_dynamic_type(), sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_array_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, size_t array_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder(), array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder(
+    get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound);
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp

@@ -0,0 +1,465 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rosidl_dynamic_typesupport/identifier.h>
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <rosidl_runtime_c/type_description_utils.h>
+#include <rosidl_runtime_c/type_description/type_description__functions.h>
+#include <rosidl_runtime_c/type_description/type_description__struct.h>
+
+#include <memory>
+#include <string>
+
+#include "rcl/allocator.h"
+#include "rcl/dynamic_message_type_support.h"
+#include "rcl/type_hash.h"
+#include "rcl/types.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/types/rcutils_ret.h"
+#include "rmw/dynamic_message_type_support.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+
+using rclcpp::dynamic_typesupport::DynamicMessage;
+using rclcpp::dynamic_typesupport::DynamicMessageType;
+using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  const std::string & serialization_library_name)
+: serialization_support_(nullptr),
+  dynamic_message_type_(nullptr),
+  dynamic_message_(nullptr),
+  description_(nullptr),
+  rosidl_message_type_support_(nullptr)
+{
+  rosidl_message_type_support_t * ts = nullptr;
+  rcl_ret_t ret;
+
+  if (serialization_library_name.empty()) {
+    ret = rcl_dynamic_message_type_support_handle_create(nullptr, &description, &ts);
+  } else {
+    ret = rcl_dynamic_message_type_support_handle_create(
+      serialization_library_name.c_str(), &description, &ts);
+  }
+  if (ret != RCL_RET_OK) {
+    throw std::runtime_error("error initializing rosidl message type support");
+  }
+  if (!ts) {
+    throw std::runtime_error("could not init rosidl message type support");
+  }
+  if (!ts->data) {
+    throw std::runtime_error("could not init rosidl message type support impl");
+  }
+  if (ts->typesupport_identifier != rosidl_get_dynamic_typesupport_identifier()) {
+    throw std::runtime_error("rosidl message type support is of the wrong type");
+  }
+
+  // NOTE(methylDragon): Not technically const correct, but since it's a const void *,
+  //                     we do it anyway...
+  auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+
+  // NOTE(methylDragon): We don't destroy the rosidl_message_type_support->data since its members
+  //                     are managed by the passed in SharedPtr wrapper classes. We just delete it.
+  rosidl_message_type_support_.reset(
+    ts,
+    [](rosidl_message_type_support_t * ts) -> void {
+      auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+      auto allocator = rcl_get_default_allocator();
+
+      // These are all C allocated
+      allocator.deallocate(ts_impl->type_hash, &allocator.state);
+      allocator.deallocate(
+        const_cast<rosidl_dynamic_message_type_support_impl_t *>(ts_impl), &allocator.state);
+      allocator.deallocate(ts, &allocator.state);
+    }
+  );
+
+  manage_description_(ts_impl->type_description);
+  serialization_support_ = DynamicSerializationSupport::make_shared(ts_impl->serialization_support);
+
+  dynamic_message_type_ = DynamicMessageType::make_shared(
+    get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type);
+
+  dynamic_message_ = DynamicMessage::make_shared(
+    get_shared_dynamic_serialization_support(), ts_impl->dynamic_message);
+
+  if (!rosidl_message_type_support_) {
+    throw std::runtime_error("could not init rosidl message type support.");
+  }
+}
+
+
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description)
+: serialization_support_(serialization_support),
+  dynamic_message_type_(nullptr),
+  dynamic_message_(nullptr),
+  description_(nullptr),
+  rosidl_message_type_support_(nullptr)
+{
+  // Check null
+  if (!serialization_support) {
+    throw std::runtime_error("serialization_support cannot be nullptr.");
+  }
+
+  rosidl_message_type_support_t * ts = nullptr;
+
+  auto type_hash = std::make_unique<rosidl_type_hash_t>();
+  rcutils_ret_t hash_ret = rcl_calculate_type_hash(
+    // TODO(methylDragon): Swap this out with the conversion function when it is ready
+    reinterpret_cast<const type_description_interfaces__msg__TypeDescription *>(&description),
+    type_hash.get());
+  if (hash_ret != RCL_RET_OK || !type_hash) {
+    throw std::runtime_error("failed to get type hash");
+  }
+
+  rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_create(
+    serialization_support->get_rosidl_serialization_support(),
+    type_hash.get(),  // type_hash
+    &description,     // type_description
+    nullptr,          // type_description_sources (not implemented for dynamic types)
+    &ts);
+  if (ret != RCUTILS_RET_OK || !ts) {
+    throw std::runtime_error("could not init rosidl message type support");
+  }
+  if (!ts->data) {
+    throw std::runtime_error("could not init rosidl message type support impl");
+  }
+  if (ts->typesupport_identifier != rosidl_get_dynamic_typesupport_identifier()) {
+    throw std::runtime_error("rosidl message type support is of the wrong type");
+  }
+
+  auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+
+  // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members
+  //                     are managed by the passed in SharedPtr wrapper classes. We just delete it.
+  rosidl_message_type_support_.reset(
+    ts,
+    [](rosidl_message_type_support_t * ts) -> void {
+      auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+
+      // These are allocated with new
+      delete ts_impl->type_hash;  // Only because we should've allocated it here
+      delete ts_impl;
+      delete ts;
+    }
+  );
+  manage_description_(ts_impl->type_description);
+
+  dynamic_message_type_ = DynamicMessageType::make_shared(
+    get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type);
+
+  dynamic_message_ = DynamicMessage::make_shared(
+    get_shared_dynamic_serialization_support(), ts_impl->dynamic_message);
+
+  if (!rosidl_message_type_support_) {
+    throw std::runtime_error("could not init rosidl message type support.");
+  }
+
+  type_hash.release();
+}
+
+
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  DynamicMessageType::SharedPtr dynamic_message_type,
+  DynamicMessage::SharedPtr dynamic_message,
+  const rosidl_runtime_c__type_description__TypeDescription & description)
+: serialization_support_(serialization_support),
+  dynamic_message_type_(dynamic_message_type),
+  dynamic_message_(dynamic_message),
+  description_(nullptr),
+  rosidl_message_type_support_(nullptr)
+{
+  // Check null
+  if (!serialization_support) {
+    throw std::runtime_error("serialization_support cannot be nullptr.");
+  }
+  if (!dynamic_message_type) {
+    throw std::runtime_error("dynamic_message_type cannot be nullptr.");
+  }
+  if (!dynamic_message) {
+    throw std::runtime_error("dynamic_message cannot be nullptr.");
+  }
+
+  description_.reset(
+    new rosidl_runtime_c__type_description__TypeDescription(),
+    [](rosidl_runtime_c__type_description__TypeDescription * description) -> void {
+      rosidl_runtime_c__type_description__TypeDescription__destroy(description);
+    });
+  if (!description_) {
+    throw std::runtime_error("could not init type description.");
+  }
+  if (!rosidl_runtime_c__type_description__TypeDescription__copy(
+      &description,
+      description_.get()))
+  {
+    throw std::runtime_error("could not copy type description.");
+  }
+
+  // Check identifiers
+  if (serialization_support->get_library_identifier() !=
+    dynamic_message_type->get_library_identifier())
+  {
+    throw std::runtime_error(
+            "serialization support library identifier does not match "
+            "dynamic message type library identifier.");
+  }
+  if (dynamic_message_type->get_library_identifier() != dynamic_message->get_library_identifier()) {
+    throw std::runtime_error(
+            "dynamic message type library identifier does not match "
+            "dynamic message library identifier.");
+  }
+
+  // Check pointers
+  /* *INDENT-OFF* */
+  if (serialization_support->get_rosidl_serialization_support() !=
+      dynamic_message_type
+        ->get_shared_dynamic_serialization_support()
+        ->get_rosidl_serialization_support())
+  {
+    throw std::runtime_error("serialization support pointer does not match dynamic message type's");
+  }
+  if (dynamic_message_type
+        ->get_shared_dynamic_serialization_support()
+        ->get_rosidl_serialization_support() !=
+      dynamic_message_type
+        ->get_shared_dynamic_serialization_support()
+        ->get_rosidl_serialization_support())
+  {
+    throw std::runtime_error("serialization support does not match pointer dynamic message type's");
+  }
+  /* *INDENT-ON* */
+
+  init_rosidl_message_type_support_(
+    serialization_support_, dynamic_message_type_, dynamic_message_, description_.get());
+  if (!rosidl_message_type_support_) {
+    throw std::runtime_error("could not init rosidl message type support.");
+  }
+}
+
+
+DynamicMessageTypeSupport::~DynamicMessageTypeSupport() {}
+
+
+void
+DynamicMessageTypeSupport::manage_description_(
+  rosidl_runtime_c__type_description__TypeDescription * description)
+{
+  if (!description) {
+    throw std::runtime_error("description cannot be nullptr");
+  }
+  description_.reset(
+    description,
+    [](rosidl_runtime_c__type_description__TypeDescription * description) -> void {
+      rosidl_runtime_c__type_description__TypeDescription__destroy(description);
+    });
+}
+
+
+// This looks like rmw_`dynamic_message_type_support_handle_create()`, but instead just aggregates
+// already initialized objects
+void
+DynamicMessageTypeSupport::init_rosidl_message_type_support_(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  DynamicMessageType::SharedPtr dynamic_message_type,
+  DynamicMessage::SharedPtr dynamic_message,
+  rosidl_runtime_c__type_description__TypeDescription * description)
+{
+  bool middleware_supports_type_discovery = rmw_feature_supported(
+    RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY);
+
+  if (!middleware_supports_type_discovery && !description) {
+    throw std::runtime_error(
+            "Middleware does not support type discovery! Deferred dynamic type"
+            "message type support will never be populated! You must provide a type "
+            "description.");
+    return;
+  }
+
+  auto type_hash = std::make_unique<rosidl_type_hash_t>();
+  rcutils_ret_t hash_ret = rcl_calculate_type_hash(
+    // TODO(methylDragon): Swap this out with the conversion function when it is ready
+    //   from https://github.com/ros2/rcl/pull/1052
+    reinterpret_cast<type_description_interfaces__msg__TypeDescription *>(description),
+    type_hash.get());
+  if (hash_ret != RCL_RET_OK || !type_hash) {
+    throw std::runtime_error("failed to get type hash");
+  }
+
+  rosidl_dynamic_message_type_support_impl_t * ts_impl =
+    new rosidl_dynamic_message_type_support_impl_t {
+    type_hash.get(),                                              // type_hash
+    description,                                                  // type_description
+    nullptr,    // NOTE(methylDragon): Not supported for now      // type_description_sources
+    serialization_support->get_rosidl_serialization_support(),    // serialization_support
+    dynamic_message_type->get_rosidl_dynamic_type(),              // dynamic_message_type
+    dynamic_message->get_rosidl_dynamic_data()                    // dynamic_message
+  };
+  if (!ts_impl) {
+    throw std::runtime_error(
+            "Could not allocate rosidl_dynamic_message_type_support_impl_t struct");
+    return;
+  }
+
+  // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members
+  //                     are managed by the passed in SharedPtr wrapper classes. We just delete it.
+  rosidl_message_type_support_.reset(
+    new rosidl_message_type_support_t{
+    rosidl_get_dynamic_typesupport_identifier(),          // typesupport_identifier
+    ts_impl,                                              // data
+    get_message_typesupport_handle_function,              // func
+    // get_type_hash_func
+    rosidl_get_dynamic_message_type_support_type_hash_function,
+    // get_type_description_func
+    rosidl_get_dynamic_message_type_support_type_description_function,
+    // get_type_description_sources_func
+    rosidl_get_dynamic_message_type_support_type_description_sources_function
+  },
+    [](rosidl_message_type_support_t * ts) -> void {
+      auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+      auto allocator = rcl_get_default_allocator();
+      // Only because we should've allocated it here (also it's C allocated)
+      allocator.deallocate(ts_impl->type_hash, &allocator.state);
+      delete ts_impl;
+    }
+  );
+
+  if (!rosidl_message_type_support_) {
+    throw std::runtime_error("Could not allocate rosidl_message_type_support_t struct");
+    delete ts_impl;
+    return;
+  }
+
+  type_hash.release();
+}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicMessageTypeSupport::get_library_identifier() const
+{
+  return serialization_support_->get_library_identifier();
+}
+
+
+rosidl_message_type_support_t *
+DynamicMessageTypeSupport::get_rosidl_message_type_support()
+{
+  return rosidl_message_type_support_.get();
+}
+
+
+const rosidl_message_type_support_t *
+DynamicMessageTypeSupport::get_rosidl_message_type_support() const
+{
+  return rosidl_message_type_support_.get();
+}
+
+
+std::shared_ptr<rosidl_message_type_support_t>
+DynamicMessageTypeSupport::get_shared_rosidl_message_type_support()
+{
+  return rosidl_message_type_support_;
+}
+
+
+std::shared_ptr<const rosidl_message_type_support_t>
+DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() const
+{
+  return rosidl_message_type_support_;
+}
+
+
+rosidl_runtime_c__type_description__TypeDescription *
+DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description()
+{
+  return description_.get();
+}
+
+
+const rosidl_runtime_c__type_description__TypeDescription *
+DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const
+{
+  return description_.get();
+}
+
+
+std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription>
+DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description()
+{
+  return description_;
+}
+
+
+std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
+DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() const
+{
+  return description_;
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+DynamicMessageType::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message_type()
+{
+  return dynamic_message_type_;
+}
+
+
+DynamicMessageType::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message_type() const
+{
+  return dynamic_message_type_;
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message()
+{
+  return dynamic_message_;
+}
+
+
+DynamicMessage::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message() const
+{
+  return dynamic_message_;
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp

@@ -0,0 +1,136 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rosidl_dynamic_typesupport/api/serialization_support.h>
+
+#include <memory>
+#include <string>
+
+#include "rcutils/logging_macros.h"
+#include "rmw/dynamic_message_type_support.h"
+#include "rmw/ret_types.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+
+
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+// CONSTRUCTION ====================================================================================
+DynamicSerializationSupport::DynamicSerializationSupport()
+: DynamicSerializationSupport::DynamicSerializationSupport("") {}
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  const std::string & serialization_library_name)
+: rosidl_serialization_support_(nullptr)
+{
+  rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support = nullptr;
+  rmw_ret_t ret = RMW_RET_ERROR;
+
+  if (serialization_library_name.empty()) {
+    ret = rmw_get_serialization_support(NULL, &rosidl_serialization_support);
+  } else {
+    ret = rmw_get_serialization_support(
+      serialization_library_name.c_str(), &rosidl_serialization_support);
+  }
+
+  if (ret != RMW_RET_OK || !rosidl_serialization_support) {
+    throw std::runtime_error("could not create new serialization support object");
+  }
+
+  rosidl_serialization_support_.reset(
+    rosidl_serialization_support,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void {
+      rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support);
+    });
+}
+
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support)
+: rosidl_serialization_support_(nullptr)
+{
+  if (!rosidl_serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+
+  // Custom deleter
+  rosidl_serialization_support_.reset(
+    rosidl_serialization_support,
+    [](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void {
+      rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support);
+    });
+}
+
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> rosidl_serialization_support)
+: rosidl_serialization_support_(rosidl_serialization_support) {}
+
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  DynamicSerializationSupport && other) noexcept
+: rosidl_serialization_support_(std::exchange(other.rosidl_serialization_support_, nullptr)) {}
+
+
+DynamicSerializationSupport &
+DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept
+{
+  std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_);
+  return *this;
+}
+
+
+DynamicSerializationSupport::~DynamicSerializationSupport() {}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicSerializationSupport::get_library_identifier() const
+{
+  return std::string(
+    rosidl_dynamic_typesupport_serialization_support_get_library_identifier(
+      rosidl_serialization_support_.get()));
+}
+
+
+rosidl_dynamic_typesupport_serialization_support_t *
+DynamicSerializationSupport::get_rosidl_serialization_support()
+{
+  return rosidl_serialization_support_.get();
+}
+
+
+const rosidl_dynamic_typesupport_serialization_support_t *
+DynamicSerializationSupport::get_rosidl_serialization_support() const
+{
+  return rosidl_serialization_support_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
+DynamicSerializationSupport::get_shared_rosidl_serialization_support()
+{
+  return std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>(
+    shared_from_this(), rosidl_serialization_support_.get());
+}
+
+
+std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>
+DynamicSerializationSupport::get_shared_rosidl_serialization_support() const
+{
+  return std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>(
+    shared_from_this(), rosidl_serialization_support_.get());
+}

rclcpp/src/rclcpp/event_handler.cpp

@@ -12,9 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <stdexcept>
 #include <string>
 
-#include "rclcpp/qos_event.hpp"
+#include "rcl/event.h"
+
+#include "rclcpp/event_handler.hpp"
+#include "rclcpp/exceptions/exceptions.hpp"
 
 namespace rclcpp
 {
@@ -33,11 +37,14 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
   std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message)
 {}
 
-QOSEventHandlerBase::~QOSEventHandlerBase()
+EventHandlerBase::~EventHandlerBase()
 {
-  if (on_new_event_callback_) {
-    clear_on_ready_callback();
-  }
+  // Since the rmw event listener holds a reference to
+  // this callback, we need to clear it on destruction of this class.
+  // This clearing is not needed for other rclcpp entities like pub/subs, since
+  // they do own the underlying rmw entities, which are destroyed
+  // on their rclcpp destructors, thus no risk of dangling pointers.
+  clear_on_ready_callback();
 
   if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
@@ -49,14 +56,14 @@ QOSEventHandlerBase::~QOSEventHandlerBase()
 
 /// Get the number of ready events.
 size_t
-QOSEventHandlerBase::get_number_of_ready_events()
+EventHandlerBase::get_number_of_ready_events()
 {
   return 1;
 }
 
 /// Add the Waitable to a wait set.
 void
-QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_);
   if (RCL_RET_OK != ret) {
@@ -66,13 +73,13 @@ QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 
 /// Check if the Waitable is ready.
 bool
-QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
+EventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
 {
   return wait_set->events[wait_set_event_index_] == &event_handle_;
 }
 
 void
-QOSEventHandlerBase::set_on_new_event_callback(
+EventHandlerBase::set_on_new_event_callback(
   rcl_event_callback_t callback,
   const void * user_data)
 {
@@ -83,7 +90,7 @@ QOSEventHandlerBase::set_on_new_event_callback(
 
   if (RCL_RET_OK != ret) {
     using rclcpp::exceptions::throw_from_rcl_error;
-    throw_from_rcl_error(ret, "failed to set the on new message callback for QOS Event");
+    throw_from_rcl_error(ret, "failed to set the on new message callback for Event");
   }
 }
 

rclcpp/src/rclcpp/executor.cpp

@@ -24,6 +24,7 @@
 #include "rcl/error_handling.h"
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/guard_condition.hpp"
@@ -106,6 +107,12 @@ 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_groups_to_guard_conditions_) {
+    auto guard_condition = pair.second;
+    memory_strategy_->remove_guard_condition(guard_condition);
+  }
+  weak_groups_to_guard_conditions_.clear();
+
   for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
@@ -204,8 +211,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 +221,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);
   }
 }
 
@@ -272,6 +281,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
       }
     });
 
+  const auto & gc = node_ptr->get_notify_guard_condition();
+  weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+  // Add the node's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(gc);
   weak_nodes_.push_back(node_ptr);
 }
 
@@ -300,7 +313,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 +328,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());
   }
 }
 
@@ -372,6 +389,9 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     }
   }
 
+  memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
+  weak_nodes_to_guard_conditions_.erase(node_ptr);
+
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   has_executor.store(false);
 }
@@ -539,13 +559,14 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   }
 }
 
+template<typename Taker, typename Handler>
 static
 void
 take_and_do_error_handling(
   const char * action_description,
   const char * topic_or_service_name,
-  std::function<bool()> take_action,
-  std::function<void()> handle_action)
+  Taker take_action,
+  Handler handle_action)
 {
   bool taken = false;
   try {
@@ -578,70 +599,136 @@ take_and_do_error_handling(
 void
 Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
 {
+  using rclcpp::dynamic_typesupport::DynamicMessage;
+
   rclcpp::MessageInfo message_info;
   message_info.get_rmw_message_info().from_intra_process = false;
 
-  if (subscription->is_serialized()) {
-    // This is the case where a copy of the serialized message is taken from
-    // the middleware via inter-process communication.
-    std::shared_ptr<SerializedMessage> serialized_msg = subscription->create_serialized_message();
-    take_and_do_error_handling(
-      "taking a serialized message from topic",
-      subscription->get_topic_name(),
-      [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
-      [&]()
+  switch (subscription->get_subscription_type()) {
+    // Take ROS message
+    case rclcpp::SubscriptionType::ROS_MESSAGE:
       {
-        subscription->handle_serialized_message(serialized_msg, message_info);
-      });
-    subscription->return_serialized_message(serialized_msg);
-  } else if (subscription->can_loan_messages()) {
-    // This is the case where a loaned message is taken from the middleware via
-    // inter-process communication, given to the user for their callback,
-    // and then returned.
-    void * loaned_msg = nullptr;
-    // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
-    //   is extened to support subscriptions as well.
-    take_and_do_error_handling(
-      "taking a loaned message from topic",
-      subscription->get_topic_name(),
-      [&]()
-      {
-        rcl_ret_t ret = rcl_take_loaned_message(
-          subscription->get_subscription_handle().get(),
-          &loaned_msg,
-          &message_info.get_rmw_message_info(),
-          nullptr);
-        if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
-          return false;
-        } else if (RCL_RET_OK != ret) {
-          rclcpp::exceptions::throw_from_rcl_error(ret);
+        if (subscription->can_loan_messages()) {
+          // This is the case where a loaned message is taken from the middleware via
+          // inter-process communication, given to the user for their callback,
+          // and then returned.
+          void * loaned_msg = nullptr;
+          // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
+          //   is extened to support subscriptions as well.
+          take_and_do_error_handling(
+            "taking a loaned message from topic",
+            subscription->get_topic_name(),
+            [&]()
+            {
+              rcl_ret_t ret = rcl_take_loaned_message(
+                subscription->get_subscription_handle().get(),
+                &loaned_msg,
+                &message_info.get_rmw_message_info(),
+                nullptr);
+              if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
+                return false;
+              } else if (RCL_RET_OK != ret) {
+                rclcpp::exceptions::throw_from_rcl_error(ret);
+              }
+              return true;
+            },
+            [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
+          if (nullptr != loaned_msg) {
+            rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
+              subscription->get_subscription_handle().get(), loaned_msg);
+            if (RCL_RET_OK != ret) {
+              RCLCPP_ERROR(
+                rclcpp::get_logger("rclcpp"),
+                "rcl_return_loaned_message_from_subscription() failed for subscription on topic "
+                "'%s': %s",
+                subscription->get_topic_name(), rcl_get_error_string().str);
+            }
+            loaned_msg = nullptr;
+          }
+        } else {
+          // This case is taking a copy of the message data from the middleware via
+          // inter-process communication.
+          std::shared_ptr<void> message = subscription->create_message();
+          take_and_do_error_handling(
+            "taking a message from topic",
+            subscription->get_topic_name(),
+            [&]() {return subscription->take_type_erased(message.get(), message_info);},
+            [&]() {subscription->handle_message(message, message_info);});
+          subscription->return_message(message);
         }
-        return true;
-      },
-      [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
-    if (nullptr != loaned_msg) {
-      rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
-        subscription->get_subscription_handle().get(),
-        loaned_msg);
-      if (RCL_RET_OK != ret) {
-        RCLCPP_ERROR(
-          rclcpp::get_logger("rclcpp"),
-          "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s",
-          subscription->get_topic_name(), rcl_get_error_string().str);
+        break;
+      }
+
+    // Take serialized message
+    case rclcpp::SubscriptionType::SERIALIZED_MESSAGE:
+      {
+        // This is the case where a copy of the serialized message is taken from
+        // the middleware via inter-process communication.
+        std::shared_ptr<SerializedMessage> serialized_msg =
+          subscription->create_serialized_message();
+        take_and_do_error_handling(
+          "taking a serialized message from topic",
+          subscription->get_topic_name(),
+          [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
+          [&]()
+          {
+            subscription->handle_serialized_message(serialized_msg, message_info);
+          });
+        subscription->return_serialized_message(serialized_msg);
+        break;
+      }
+
+    // DYNAMIC SUBSCRIPTION ========================================================================
+    // If a subscription is dynamic, then it will use its serialization-specific dynamic data.
+    //
+    // Two cases:
+    // - Dynamic type subscription using dynamic type stored in its own internal type support struct
+    // - Non-dynamic type subscription with no stored dynamic type
+    //   - Subscriptions of this type must be able to lookup the local message description to
+    //     generate a dynamic type at runtime!
+    //   - TODO(methylDragon): I won't be handling this case yet
+
+    // Take dynamic message directly from the middleware
+    case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT:
+      {
+        DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message();
+        take_and_do_error_handling(
+          "taking a dynamic message from topic",
+          subscription->get_topic_name(),
+          // This modifies the stored dynamic data in the DynamicMessage in-place
+          [&]() {return subscription->take_dynamic_message(*dynamic_message, message_info);},
+          [&]() {subscription->handle_dynamic_message(dynamic_message, message_info);});
+        subscription->return_dynamic_message(dynamic_message);
+        break;
+      }
+
+    // Take serialized and then convert to dynamic message
+    case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED:
+      {
+        std::shared_ptr<SerializedMessage> serialized_msg =
+          subscription->create_serialized_message();
+
+        // NOTE(methylDragon): Is this clone necessary? If I'm following the pattern, it seems so.
+        DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message();
+        take_and_do_error_handling(
+          "taking a serialized message from topic",
+          subscription->get_topic_name(),
+          [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
+          [&]()
+          {
+            bool ret = dynamic_message->deserialize(&serialized_msg->get_rcl_serialized_message());
+            if (!ret) {
+              throw_from_rcl_error(ret, "Couldn't convert serialized message to dynamic data!");
+            }
+            subscription->handle_dynamic_message(dynamic_message, message_info);
+          }
+        );
+        subscription->return_serialized_message(serialized_msg);
+        subscription->return_dynamic_message(dynamic_message);
+        break;
       }
-      loaned_msg = nullptr;
-    }
-  } else {
-    // This case is taking a copy of the message data from the middleware via
-    // inter-process communication.
-    std::shared_ptr<void> message = subscription->create_message();
-    take_and_do_error_handling(
-      "taking a message from topic",
-      subscription->get_topic_name(),
-      [&]() {return subscription->take_type_erased(message.get(), message_info);},
-      [&]() {subscription->handle_message(message, message_info);});
-    subscription->return_message(message);
   }
+  return;
 }
 
 void
@@ -721,6 +808,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);
         });
     }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -21,6 +21,7 @@
 
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/logging.hpp"
 #include "rclcpp/utilities.hpp"
 
 using rclcpp::executors::MultiThreadedExecutor;
@@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor(
   yield_before_execute_(yield_before_execute),
   next_exec_timeout_(next_exec_timeout)
 {
-  number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
-  if (number_of_threads_ == 0) {
-    number_of_threads_ = 1;
+  number_of_threads_ = number_of_threads > 0 ?
+    number_of_threads :
+    std::max(std::thread::hardware_concurrency(), 2U);
+
+  if (number_of_threads_ == 1) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "MultiThreadedExecutor is used with a single thread.\n"
+      "Use the SingleThreadedExecutor instead.");
   }
 }
 

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -43,8 +43,7 @@ void GenericSubscription::handle_message(
           "handle_message is not implemented for GenericSubscription");
 }
 
-void
-GenericSubscription::handle_serialized_message(
+void GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
   const rclcpp::MessageInfo &)
 {
@@ -72,4 +71,55 @@ void GenericSubscription::return_serialized_message(
   message.reset();
 }
 
+
+// DYNAMIC TYPE ====================================================================================
+// TODO(methylDragon): Reorder later
+rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+GenericSubscription::get_shared_dynamic_message_type()
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "get_shared_dynamic_message_type is not implemented for GenericSubscription");
+}
+
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+GenericSubscription::get_shared_dynamic_message()
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "get_shared_dynamic_message is not implemented for GenericSubscription");
+}
+
+rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+GenericSubscription::get_shared_dynamic_serialization_support()
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "get_shared_dynamic_serialization_support is not implemented for GenericSubscription");
+}
+
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+GenericSubscription::create_dynamic_message()
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "create_dynamic_message is not implemented for GenericSubscription");
+}
+
+void
+GenericSubscription::return_dynamic_message(
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
+{
+  (void) message;
+  throw rclcpp::exceptions::UnimplementedError(
+          "return_dynamic_message is not implemented for GenericSubscription");
+}
+
+void
+GenericSubscription::handle_dynamic_message(
+  const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+  const rclcpp::MessageInfo & message_info)
+{
+  (void) message;
+  (void) message_info;
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_dynamic_message is not implemented for GenericSubscription");
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/graph_listener.cpp

@@ -48,7 +48,7 @@ GraphListener::GraphListener(const std::shared_ptr<Context> & parent_context)
 
 GraphListener::~GraphListener()
 {
-  this->shutdown(std::nothrow);
+  GraphListener::shutdown(std::nothrow);
 }
 
 void GraphListener::init_wait_set()

rclcpp/src/rclcpp/guard_condition.cpp

@@ -74,16 +74,19 @@ GuardCondition::get_rcl_guard_condition() const
 void
 GuardCondition::trigger()
 {
-  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
 
-  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);
+  {
+    std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+    if (on_trigger_callback_) {
+      on_trigger_callback_(1);
+    } else {
+      unread_count_++;
     }
-    unread_count_++;
   }
 }
 
@@ -125,10 +128,9 @@ GuardCondition::set_on_trigger_callback(std::function<void(size_t)> callback)
       callback(unread_count_);
       unread_count_ = 0;
     }
-    return;
+  } else {
+    on_trigger_callback_ = nullptr;
   }
-
-  on_trigger_callback_ = nullptr;
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/logger.cpp

@@ -12,14 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
 #include <string>
+#include <utility>
 
 #include "rcl_logging_interface/rcl_logging_interface.h"
+#include "rcl/logging_rosout.h"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 
+#include "./logging_mutex.hpp"
+
 namespace rclcpp
 {
 
@@ -62,6 +67,46 @@ get_logging_directory()
   return path;
 }
 
+Logger
+Logger::get_child(const std::string & suffix)
+{
+  if (!name_) {
+    return Logger();
+  }
+
+  rcl_ret_t rcl_ret = RCL_RET_OK;
+  std::shared_ptr<std::recursive_mutex> logging_mutex;
+  logging_mutex = get_global_logging_mutex();
+  {
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+    rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str());
+    if (RCL_RET_OK != rcl_ret) {
+      exceptions::throw_from_rcl_error(
+        rcl_ret, "failed to call rcl_logging_rosout_add_sublogger",
+        rcutils_get_error_state(), rcutils_reset_error);
+    }
+  }
+
+  Logger logger(*name_ + RCUTILS_LOGGING_SEPARATOR_STRING + suffix);
+  if (RCL_RET_OK == rcl_ret) {
+    logger.logger_sublogger_pairname_.reset(
+      new std::pair<std::string, std::string>({*name_, suffix}),
+      [logging_mutex](std::pair<std::string, std::string> * logger_sublogger_pairname_ptr) {
+        std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+        rcl_ret_t rcl_ret = rcl_logging_rosout_remove_sublogger(
+          logger_sublogger_pairname_ptr->first.c_str(),
+          logger_sublogger_pairname_ptr->second.c_str());
+        delete logger_sublogger_pairname_ptr;
+        if (RCL_RET_OK != rcl_ret) {
+          exceptions::throw_from_rcl_error(
+            rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger",
+            rcutils_get_error_state(), rcutils_reset_error);
+        }
+      });
+  }
+  return logger;
+}
+
 void
 Logger::set_level(Level level)
 {
@@ -80,4 +125,18 @@ Logger::set_level(Level level)
   }
 }
 
+Logger::Level
+Logger::get_effective_level() const
+{
+  int logger_level = rcutils_logging_get_logger_effective_level(get_name());
+
+  if (logger_level < 0) {
+    exceptions::throw_from_rcl_error(
+      RCL_RET_ERROR, "Couldn't get logger level",
+      rcutils_get_error_state(), rcutils_reset_error);
+  }
+
+  return static_cast<Level>(logger_level);
+}
+
 }  // namespace rclcpp