Take ownership of description, add print, and update ser support API

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,37 @@
 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>`_)

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_data.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_type.cpp
+  src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions/exceptions.cpp
   src/rclcpp/executable_list.cpp
@@ -92,7 +98,7 @@ 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/serialization.cpp
   src/rclcpp/serialized_message.cpp

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

@@ -965,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/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"
@@ -467,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) {
@@ -781,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<
@@ -831,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/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>
 {
@@ -376,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).
@@ -398,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/dynamic_typesupport/detail/dynamic_data_impl.hpp

@@ -0,0 +1,326 @@
+// 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_DATA_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_data.hpp"
+#endif
+
+
+#define __DYNAMIC_DATA_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicData::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_DATA_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicData::get_value<ValueT>(const std::string & name) \
+  { \
+    return get_value<ValueT>(get_member_id(name)); \
+  }
+
+#define __DYNAMIC_DATA_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicData::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_DATA_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicData::set_value<ValueT>(const std::string & name, ValueT value) \
+  { \
+    set_value<ValueT>(get_member_id(name), value); \
+  }
+
+#define __DYNAMIC_DATA_INSERT_VALUE(ValueT, FunctionT) \
+  template<> \
+  rosidl_dynamic_typesupport_member_id_t \
+  DynamicData::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_DATA_DEFINITIONS(ValueT, FunctionT) \
+  __DYNAMIC_DATA_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_DATA_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_DATA_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_DATA_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_DATA_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_DATA_DEFINITIONS(bool, bool);
+// DYNAMIC_DATA_DEFINITIONS(std::byte, byte);
+DYNAMIC_DATA_DEFINITIONS(char, char);
+DYNAMIC_DATA_DEFINITIONS(float, float32);
+DYNAMIC_DATA_DEFINITIONS(double, float64);
+DYNAMIC_DATA_DEFINITIONS(int8_t, int8);
+DYNAMIC_DATA_DEFINITIONS(int16_t, int16);
+DYNAMIC_DATA_DEFINITIONS(int32_t, int32);
+DYNAMIC_DATA_DEFINITIONS(int64_t, int64);
+DYNAMIC_DATA_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_DATA_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_DATA_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_DATA_DEFINITIONS(uint64_t, uint64);
+// DYNAMIC_DATA_DEFINITIONS(std::string, std::string);
+// DYNAMIC_DATA_DEFINITIONS(std::u16string, std::u16string);
+
+// Byte and String getters have a different implementation and are defined below
+
+
+// BYTE ============================================================================================
+template<>
+std::byte
+DynamicData::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
+DynamicData::get_value<std::byte>(const std::string & name)
+{
+  return get_value<std::byte>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::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);
+  free(buf);
+  return out;
+}
+
+
+template<>
+std::u16string
+DynamicData::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);
+  free(buf);
+  return out;
+}
+
+
+template<>
+std::string
+DynamicData::get_value<std::string>(const std::string & name)
+{
+  return get_value<std::string>(get_member_id(name));
+}
+
+
+template<>
+std::u16string
+DynamicData::get_value<std::u16string>(const std::string & name)
+{
+  return get_value<std::u16string>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicData::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
+DynamicData::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
+DynamicData::set_value<std::string>(const std::string & name, const std::string value)
+{
+  set_value<std::string>(get_member_id(name), value);
+}
+
+
+template<>
+void
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::get_value(const std::string & name)
+{
+  throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+void
+DynamicData::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
+DynamicData::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
+DynamicData::insert_value(ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_DATA_GET_VALUE_BY_ID_FN
+#undef __DYNAMIC_DATA_GET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_DATA_SET_VALUE_BY_ID_FN
+#undef __DYNAMIC_DATA_SET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_DATA_INSERT_VALUE
+#undef DYNAMIC_DATA_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp

@@ -0,0 +1,162 @@
+// 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_TYPE_BUILDER_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#endif
+
+
+#define __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicTypeBuilder::add_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \
+      rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size()); \
+  }
+
+#define __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicTypeBuilder::add_array_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \
+      rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size(), array_length); \
+  }
+
+#define __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicTypeBuilder::add_unbounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _unbounded_sequence_member( \
+      rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size()); \
+  }
+
+#define __DYNAMIC_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicTypeBuilder::add_bounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \
+      rosidl_dynamic_type_builder_.get(), id, name.c_str(), name.size(), sequence_bound); \
+  }
+
+#define DYNAMIC_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \
+  __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_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_TYPE_BUILDER_DEFINITIONS(bool, bool);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::byte, byte);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(char, char);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(float, float32);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(double, float64);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(int8_t, int8);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(int16_t, int16);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(int32_t, int32);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(int64_t, int64);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::string, string);
+DYNAMIC_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring);
+
+
+// THROW FOR UNSUPPORTED TYPES =====================================================================
+template<typename MemberT>
+void
+DynamicTypeBuilder::add_member(rosidl_dynamic_typesupport_member_id_t id, const std::string & name)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+    "add_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicTypeBuilder::add_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+    "add_array_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicTypeBuilder::add_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+    "add_unbounded_sequence_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicTypeBuilder::add_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+    "add_bounded_sequence_member is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_TYPE_BUILDER_ADD_MEMBER_FN
+#undef __DYNAMIC_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN
+#undef __DYNAMIC_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN
+#undef __DYNAMIC_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN
+#undef DYNAMIC_TYPE_BUILDER_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_data.hpp

@@ -0,0 +1,374 @@
+// 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_DATA_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_DATA_HPP_
+
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rcl/types.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+class DynamicType;
+class DynamicTypeBuilder;
+
+/// 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!
+ */
+class DynamicData : public std::enable_shared_from_this<DynamicData>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicData)
+
+  // 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 DynamicData with the provided dynamic type builder
+  RCLCPP_PUBLIC
+  explicit DynamicData(std::shared_ptr<DynamicTypeBuilder> dynamic_type_builder);
+
+  /// Construct a new DynamicData with the provided dynamic type
+  RCLCPP_PUBLIC
+  explicit DynamicData(std::shared_ptr<DynamicType> dynamic_type);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicData(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicData(
+    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
+  DynamicData(
+    DynamicData::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
+  DynamicData(const DynamicData & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicData(DynamicData && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicData & operator=(const DynamicData & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicData & operator=(DynamicData && other) noexcept;
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicData();
+
+
+  // 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
+  DynamicData
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicData::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicData & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicData::SharedPtr
+  loan_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicData::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
+  void
+  print() const;
+
+  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);
+
+
+  // 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
+  DynamicData
+  get_complex_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicData
+  get_complex_value(const std::string & name);
+
+  RCLCPP_PUBLIC
+  DynamicData::SharedPtr
+  get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  DynamicData::SharedPtr
+  get_complex_value_shared(const std::string & name);
+
+  RCLCPP_PUBLIC
+  void
+  set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicData & value);
+
+  RCLCPP_PUBLIC
+  void
+  set_complex_value(const std::string & name, DynamicData & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_complex_value_copy(const DynamicData & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_complex_value(DynamicData & 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_;
+  DynamicData::SharedPtr parent_data_;  // Used for returning the loaned value, and lifetime management
+
+private:
+  RCLCPP_PUBLIC
+  DynamicData();
+
+  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_DATA_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -0,0 +1,46 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_data.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+// NOTE(methylDragon): We just alias the type in this case...
+//                     I'd have made a wrapper class but then I'd need to redirect every single
+//                     method (or dynamic cast everywhere else), so.. no thanks.
+using DynamicMessage = DynamicData;
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp

@@ -0,0 +1,45 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+// NOTE(methylDragon): We just alias the type in this case...
+//                     I'd have made a wrapper class but then I'd need to redirect every single
+//                     method (or dynamic cast everywhere else), so.. no thanks.
+using DynamicMessageType = DynamicType;
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -0,0 +1,195 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#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>
+
+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_get_dynamic_message_typesupport_handle,
+ *       because this class will manage the lifetimes for you.
+ *
+ *       Do NOT call rcl_dynamic_message_typesupport_handle_fini!!
+ *
+ * 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
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    rosidl_runtime_c__type_description__TypeDescription * description,
+    const std::string & serialization_library_name = "");
+
+  /// From description, for provided serialization support
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_runtime_c__type_description__TypeDescription * description);
+
+  /// Assume ownership of managed types
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    DynamicMessageType::SharedPtr dynamic_message_type,
+    DynamicMessage::SharedPtr dynamic_message,
+    rosidl_runtime_c__type_description__TypeDescription * description = nullptr);
+
+  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;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  void
+  print_description() 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
+  init_serialization_support_(const std::string & serialization_library_name);
+
+  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_(DynamicType::SharedPtr dynamic_type);
+
+  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,110 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+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 ==================================================================================
+  /// 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_;
+
+private:
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport();
+};
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type.hpp

@@ -0,0 +1,196 @@
+// 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_TYPE_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+
+class DynamicData;
+class DynamicTypeBuilder;
+
+/// 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!
+ */
+class DynamicType : public std::enable_shared_from_this<DynamicType>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicType)
+
+  // 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 DynamicType with the provided dynamic type builder
+  RCLCPP_PUBLIC
+  explicit DynamicType(std::shared_ptr<DynamicTypeBuilder> dynamic_type_builder);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type);
+
+  /// From description
+  RCLCPP_PUBLIC
+  DynamicType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription * description);
+
+  /// Copy constructor
+  RCLCPP_PUBLIC
+  DynamicType(const DynamicType & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicType(DynamicType && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicType & operator=(const DynamicType & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicType & operator=(DynamicType && other) noexcept;
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicType();
+
+  /// 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
+  DynamicType
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicType::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicType & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicData
+  build_data();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<DynamicData>
+  build_data_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
+  DynamicType();
+
+  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_TYPE_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_type_builder.hpp

@@ -0,0 +1,342 @@
+// 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_TYPE_BUILDER_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_TYPE_BUILDER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include <rosidl_dynamic_typesupport/types.h>
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+class DynamicData;
+class DynamicType;
+
+/// 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!
+ */
+class DynamicTypeBuilder : public std::enable_shared_from_this<DynamicTypeBuilder>
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicTypeBuilder)
+
+  // 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 DynamicTypeBuilder with the provided serialization support
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const std::string & name);
+
+  /// Assume ownership of raw pointer
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_type_builder_t * dynamic_type_builder);
+
+  /// Copy shared pointer
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> dynamic_type_builder);
+
+  /// Copy constructor
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(const DynamicTypeBuilder & other);
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(DynamicTypeBuilder && other) noexcept;
+
+  /// Copy assignment
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder & operator=(const DynamicTypeBuilder & other);
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder & operator=(DynamicTypeBuilder && other) noexcept;
+
+  /// From description
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription * description);
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicTypeBuilder();
+
+  /// 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
+  DynamicTypeBuilder
+  clone() const;
+
+  RCLCPP_PUBLIC
+  DynamicTypeBuilder::SharedPtr
+  clone_shared() const;
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  DynamicData
+  build_data();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<DynamicData>
+  build_data_shared();
+
+  RCLCPP_PUBLIC
+  DynamicType
+  build_type();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<DynamicType>
+  build_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);
+
+  template<typename MemberT>
+  void
+  add_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length);
+
+  template<typename MemberT>
+  void
+  add_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name);
+
+  template<typename MemberT>
+  void
+  add_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound);
+
+
+  // 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);
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound);
+
+  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);
+
+  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);
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_string_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  void
+  add_bounded_wstring_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound);
+
+  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);
+
+  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);
+
+
+  // ADD NESTED MEMBERS ============================================================================
+  RCLCPP_PUBLIC
+  void
+  add_complex_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicType & nested_type);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_array_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicType & nested_type, size_t array_length);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_unbounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicType & nested_type);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_bounded_sequence_member(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicType & nested_type, size_t sequence_bound);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicTypeBuilder & nested_type_builder);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_array_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicTypeBuilder & nested_type_builder, size_t array_length);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_unbounded_sequence_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicTypeBuilder & nested_type_builder);
+
+  RCLCPP_PUBLIC
+  void
+  add_complex_bounded_sequence_member_builder(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+    DynamicTypeBuilder & nested_type_builder, size_t sequence_bound);
+
+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
+  DynamicTypeBuilder();
+
+  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_TYPE_BUILDER_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(
@@ -560,11 +660,20 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
+  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+      const rclcpp::GuardCondition *,
+      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_);

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

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

@@ -44,6 +44,10 @@ struct NodeInterfacesStorage
   : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
   {}
 
+  NodeInterfacesStorage()
+  : interfaces_()
+  {}
+
   explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
   : interfaces_(args ...)
   {}

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -147,10 +147,9 @@ public:
   : NodeInterfacesSupportsT(node)
   {}
 
-  /// NodeT::SharedPtr Constructor
-  template<typename NodeT>
-  NodeInterfaces(std::shared_ptr<NodeT> node)  // NOLINT(runtime/explicit)
-  : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr"))
+  // Create a NodeInterfaces object with no bound interfaces
+  NodeInterfaces()
+  : NodeInterfacesSupportsT()
   {}
 
   explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)

rclcpp/include/rclcpp/publisher.hpp

@@ -381,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
@@ -398,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"
@@ -124,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
@@ -276,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
@@ -327,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,
@@ -339,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>;

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

rclcpp/include/rclcpp/qos.hpp

@@ -80,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
@@ -97,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.

rclcpp/include/rclcpp/qos_event.hpp

@@ -15,278 +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<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);
-
-  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,22 @@
  * - Allocator related items:
  *   - rclcpp/allocator/allocator_common.hpp
  *   - rclcpp/allocator/allocator_deleter.hpp
+ * - Dynamic typesupport wrappers
+ *   - rclcpp::dynamic_typesupport::DynamicData
+ *   - rclcpp::dynamic_typesupport::DynamicMessage
+ *   - rclcpp::dynamic_typesupport::DynamicMessageType
+ *   - rclcpp::dynamic_typesupport::DynamicSerializationSupport
+ *   - rclcpp::dynamic_typesupport::DynamicTypeBuilder
+ *   - rclcpp::dynamic_typesupport::DynamicType
+ *   - rclcpp/dynamic_typesupport/dynamic_data.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_message.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_message_type.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_type_builder.hpp
+ *   - rclcpp/dynamic_typesupport/dynamic_type.hpp
+ * - Dynamic typesupport
+ *   - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
+ *   - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
  * - Generic publisher
  *   - rclcpp::Node::create_generic_publisher()
  *   - rclcpp::GenericPublisher

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"
@@ -308,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)
@@ -331,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) {
@@ -371,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())) {
@@ -406,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)) {
@@ -487,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_base.hpp

@@ -37,7 +37,7 @@
 #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"
@@ -115,7 +115,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.
@@ -457,7 +457,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
@@ -542,7 +542,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,
@@ -555,6 +555,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;
@@ -571,7 +574,7 @@ protected:
   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_;
@@ -588,7 +591,7 @@ private:
 
   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"

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);
 };
 
 
@@ -190,10 +227,16 @@ public:
       rclcpp_timer_callback_added,
       static_cast<const void *>(get_timer_handle().get()),
       reinterpret_cast<const void *>(&callback_));
-    TRACEPOINT(
-      rclcpp_callback_register,
-      reinterpret_cast<const void *>(&callback_),
-      tracetools::get_symbol(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.

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,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>18.0.0</version>
+  <version>19.3.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
@@ -39,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/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"
 
@@ -241,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/context.cpp

@@ -219,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();
@@ -365,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;
@@ -415,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/dynamic_typesupport/dynamic_data.cpp

@@ -0,0 +1,641 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_data.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rcl/types.h"
+#include "rcutils/logging_macros.h"
+
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+using rclcpp::dynamic_typesupport::DynamicData;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+using rclcpp::dynamic_typesupport::DynamicType;
+using rclcpp::dynamic_typesupport::DynamicTypeBuilder;
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_DATA_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_data_impl.hpp"
+#endif
+
+
+// CONSTRUCTION ==================================================================================
+DynamicData::DynamicData(const DynamicTypeBuilder::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;
+  rosidl_dynamic_data = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder(
+    rosidl_dynamic_type_builder);
+  if (!rosidl_dynamic_data) {
+    throw std::runtime_error("could not create new dynamic data object");
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_dynamic_data,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      rosidl_dynamic_typesupport_dynamic_data_fini(rosidl_dynamic_data);
+      free(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicData::DynamicData(const DynamicType::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;
+  rosidl_dynamic_data = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type(
+    rosidl_dynamic_type);
+  if (!rosidl_dynamic_data) {
+    throw std::runtime_error("could not create new dynamic data object");
+  }
+
+  rosidl_dynamic_data_.reset(
+    rosidl_dynamic_data,
+    // Custom deleter
+    [](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
+      rosidl_dynamic_typesupport_dynamic_data_fini(rosidl_dynamic_data);
+      free(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicData::DynamicData(
+  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_fini(rosidl_dynamic_data);
+      free(rosidl_dynamic_data);
+    });
+}
+
+
+DynamicData::DynamicData(
+  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!");
+    }
+  }
+}
+
+
+DynamicData::DynamicData(
+  DynamicData::SharedPtr parent_data,
+  rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data)
+: DynamicData(parent_data->get_shared_dynamic_serialization_support(), rosidl_loaned_data)
+{
+  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!");
+  }
+
+  parent_data_ = parent_data;
+  is_loaned_ = true;
+}
+
+
+DynamicData::DynamicData(const DynamicData & other)
+: enable_shared_from_this(),
+  serialization_support_(nullptr),
+  rosidl_dynamic_data_(nullptr),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  DynamicData 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_);
+}
+
+
+DynamicData::DynamicData(DynamicData && 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))
+{}
+
+
+DynamicData &
+DynamicData::operator=(const DynamicData & other)
+{
+  return *this = DynamicData(other);
+}
+
+
+DynamicData &
+DynamicData::operator=(DynamicData && 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;
+}
+
+
+DynamicData::~DynamicData()
+{
+  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
+DynamicData::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
+DynamicData::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_data_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicData::get_name() const
+{
+  size_t buf_length;
+  const char * buf = rosidl_dynamic_typesupport_dynamic_data_get_name(
+    get_rosidl_dynamic_data(), &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_data_t *
+DynamicData::get_rosidl_dynamic_data()
+{
+  return rosidl_dynamic_data_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_data_t *
+DynamicData::get_rosidl_dynamic_data() const
+{
+  return rosidl_dynamic_data_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
+DynamicData::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>
+DynamicData::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
+DynamicData::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicData::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+size_t
+DynamicData::get_item_count() const
+{
+  return rosidl_dynamic_typesupport_dynamic_data_get_item_count(get_rosidl_dynamic_data());
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::get_member_id(size_t index) const
+{
+  return rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index(
+    get_rosidl_dynamic_data(), index);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::get_member_id(const std::string & name) const
+{
+  return rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name(
+    get_rosidl_dynamic_data(), name.c_str(), name.size());
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::get_array_index(size_t index) const
+{
+  return rosidl_dynamic_typesupport_dynamic_data_get_array_index(
+    get_rosidl_dynamic_data(), index);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::get_array_index(const std::string & name) const
+{
+  return get_array_index(get_member_id(name));
+}
+
+
+// METHODS =======================================================================================
+DynamicData
+DynamicData::clone() const
+{
+  return DynamicData(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_data_clone(get_rosidl_dynamic_data()));
+}
+
+
+DynamicData::SharedPtr
+DynamicData::clone_shared() const
+{
+  return DynamicData::make_shared(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_data_clone(get_rosidl_dynamic_data()));
+}
+
+
+bool
+DynamicData::equals(const DynamicData & other) const
+{
+  if (get_library_identifier() != other.get_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  return rosidl_dynamic_typesupport_dynamic_data_equals(
+    get_rosidl_dynamic_data(), other.get_rosidl_dynamic_data());
+}
+
+
+DynamicData::SharedPtr
+DynamicData::loan_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  return DynamicData::make_shared(
+    shared_from_this(),
+    rosidl_dynamic_typesupport_dynamic_data_loan_value(
+      get_rosidl_dynamic_data(), id));
+}
+
+
+DynamicData::SharedPtr
+DynamicData::loan_value(const std::string & name)
+{
+  return loan_value(get_member_id(name));
+}
+
+
+void
+DynamicData::clear_all_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_all_values(get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicData::clear_nonkey_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicData::clear_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_value(get_rosidl_dynamic_data(), id);
+}
+
+
+void
+DynamicData::clear_value(const std::string & name)
+{
+  clear_value(get_member_id(name));
+}
+
+
+void
+DynamicData::clear_sequence()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(get_rosidl_dynamic_data());
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::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
+DynamicData::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index)
+{
+  rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data(
+    get_rosidl_dynamic_data(), index);
+}
+
+
+void
+DynamicData::print() const
+{
+  rosidl_dynamic_typesupport_dynamic_data_print(get_rosidl_dynamic_data());
+}
+
+
+bool
+DynamicData::serialize(rcl_serialized_message_t * buffer)
+{
+  return rosidl_dynamic_typesupport_dynamic_data_serialize(get_rosidl_dynamic_data(), buffer);
+}
+
+
+bool
+DynamicData::deserialize(rcl_serialized_message_t * buffer)
+{
+  return rosidl_dynamic_typesupport_dynamic_data_deserialize(get_rosidl_dynamic_data(), buffer);
+}
+
+
+// MEMBER ACCESS ===================================================================================
+// Defined in "detail/dynamic_data_impl.hpp"
+
+
+// BOUNDED STRING MEMBER ACCESS ====================================================================
+const std::string
+DynamicData::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);
+  free(buf);
+  return out;
+}
+
+
+const std::string
+DynamicData::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
+DynamicData::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);
+  free(buf);
+  return out;
+}
+
+
+const std::u16string
+DynamicData::get_bounded_wstring_value(const std::string & name, size_t wstring_bound)
+{
+  return get_bounded_wstring_value(get_member_id(name), wstring_bound);
+}
+
+
+void
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::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
+DynamicData::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 ============================================================================
+DynamicData
+DynamicData::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 DynamicData(get_shared_dynamic_serialization_support(), out_ptr);
+}
+
+
+DynamicData
+DynamicData::get_complex_value(const std::string & name)
+{
+  return get_complex_value(get_member_id(name));
+}
+
+
+DynamicData::SharedPtr
+DynamicData::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 DynamicData::make_shared(get_shared_dynamic_serialization_support(), out_ptr);
+}
+
+
+DynamicData::SharedPtr
+DynamicData::get_complex_value_shared(const std::string & name)
+{
+  return get_complex_value_shared(get_member_id(name));
+}
+
+
+void
+DynamicData::set_complex_value(
+  rosidl_dynamic_typesupport_member_id_t id, DynamicData & value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_complex_value(
+    get_rosidl_dynamic_data(), id, value.get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicData::set_complex_value(const std::string & name, DynamicData & value)
+{
+  set_complex_value(get_member_id(name), value);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicData::insert_complex_value_copy(const DynamicData & 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
+DynamicData::insert_complex_value(DynamicData & 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_support.cpp

@@ -0,0 +1,396 @@
+// 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 <memory>
+#include <string>
+
+#include "rmw/dynamic_typesupport.h"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
+
+#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rcutils/logging_macros.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>
+
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+using rclcpp::dynamic_typesupport::DynamicMessage;
+using rclcpp::dynamic_typesupport::DynamicMessageType;
+using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
+
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  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)
+{
+  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);
+    });
+
+  init_serialization_support_(serialization_library_name);
+  if (!serialization_support_) {
+    throw std::runtime_error("could not init dynamic serialization support!");
+  }
+
+  init_dynamic_message_type_(serialization_support_, description);
+  if (!dynamic_message_type_) {
+    throw std::runtime_error("could not init dynamic message type!");
+  }
+
+  init_dynamic_message_(dynamic_message_type_);
+  if (!dynamic_message_) {
+    throw std::runtime_error("could not init dynamic message!");
+  }
+
+  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(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  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!");
+  }
+  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);
+    });
+
+  // Init
+  init_dynamic_message_type_(serialization_support_, description);
+  if (!dynamic_message_type_) {
+    throw std::runtime_error("could not init dynamic message type!");
+  }
+
+  init_dynamic_message_(dynamic_message_type_);
+  if (!dynamic_message_) {
+    throw std::runtime_error("could not init dynamic message!");
+  }
+
+  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(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  DynamicMessageType::SharedPtr dynamic_message_type,
+  DynamicMessage::SharedPtr dynamic_message,
+  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!");
+  }
+
+  if (description) {
+    description_.reset(
+      description,
+      [](rosidl_runtime_c__type_description__TypeDescription * description) -> void {
+        rosidl_runtime_c__type_description__TypeDescription__destroy(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 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 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::init_serialization_support_(
+  const std::string & serialization_library_name)
+{
+  serialization_support_ = DynamicSerializationSupport::make_shared(serialization_library_name);
+}
+
+
+void
+DynamicMessageTypeSupport::init_dynamic_message_type_(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription * description)
+{
+  dynamic_message_type_ = DynamicMessageType::make_shared(serialization_support, description);
+}
+
+
+void
+DynamicMessageTypeSupport::init_dynamic_message_(DynamicType::SharedPtr dynamic_type)
+{
+  dynamic_message_ = DynamicMessage::make_shared(dynamic_type);
+}
+
+
+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);
+  bool middleware_can_take_dynamic_data =
+    rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_DATA);
+
+  if (!middleware_supports_type_discovery && !description) {
+    RCUTILS_LOG_ERROR_NAMED(
+      rmw_dynamic_typesupport_c__identifier,
+      "Middleware does not support type discovery! Deferred dynamic type"
+      "message type support will never be populated! You must provide a type "
+      "description!");
+    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
+  rosidl_message_type_support_.reset(
+    new rosidl_message_type_support_t(),
+    [](rosidl_message_type_support_t * ts) -> void {free(const_cast<void *>(ts->data));}
+  );
+
+  if (!rosidl_message_type_support_) {
+    RCUTILS_LOG_ERROR_NAMED(
+      rmw_dynamic_typesupport_c__identifier,
+      "Could not allocate rosidl_message_type_support_t struct");
+    return;
+  }
+
+  rosidl_message_type_support_->typesupport_identifier = rmw_dynamic_typesupport_c__identifier;
+
+  // NOTE(methylDragon): To populate dynamic_type and description if deferred, OUTSIDE
+  rosidl_message_type_support_->data = calloc(1, sizeof(rmw_dynamic_typesupport_impl_t));
+  if (!rosidl_message_type_support_->data) {
+    RCUTILS_LOG_ERROR_NAMED(
+      rmw_dynamic_typesupport_c__identifier,
+      "Could not allocate rmw_dynamic_typesupport_impl_t struct");
+    rosidl_message_type_support_.reset();
+  }
+
+  rmw_dynamic_typesupport_impl_t * ts_impl =
+    (rmw_dynamic_typesupport_impl_t *)rosidl_message_type_support_->data;
+
+  ts_impl->take_dynamic_data = middleware_can_take_dynamic_data;
+  ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support();
+  ts_impl->dynamic_type = dynamic_message_type->get_rosidl_dynamic_type();
+  ts_impl->dynamic_data = dynamic_message->get_rosidl_dynamic_data();
+  ts_impl->description = description;
+  rosidl_message_type_support_->func = get_message_typesupport_handle_function;
+}
+
+
+// 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_;
+}
+
+
+// METHODS =========================================================================================
+void
+DynamicMessageTypeSupport::print_description() const
+{
+  if (!description_) {
+    RCUTILS_LOG_ERROR("Can't print description, no bound description!");
+  }
+  rosidl_runtime_c_type_description_utils_print_type_description(description_.get());
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp

@@ -0,0 +1,129 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rcutils/logging_macros.h"
+#include "rmw/dynamic_typesupport.h"
+
+#include <rosidl_dynamic_typesupport/api/serialization_support.h>
+
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+
+// CONSTRUCTION ====================================================================================
+DynamicSerializationSupport::DynamicSerializationSupport(
+  const std::string & serialization_library_name)
+: rosidl_serialization_support_(nullptr)
+{
+  rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support = nullptr;
+
+  if (serialization_library_name.empty()) {
+    rosidl_serialization_support = rmw_get_serialization_support(NULL);
+  } else {
+    rosidl_serialization_support =
+      rmw_get_serialization_support(serialization_library_name.c_str());
+  }
+
+  if (!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/dynamic_typesupport/dynamic_type.cpp

@@ -0,0 +1,320 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_data.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rcutils/logging_macros.h"
+
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+using rclcpp::dynamic_typesupport::DynamicData;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+using rclcpp::dynamic_typesupport::DynamicType;
+using rclcpp::dynamic_typesupport::DynamicTypeBuilder;
+
+
+// CONSTRUCTION ====================================================================================
+DynamicType::DynamicType(DynamicTypeBuilder::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;
+  rosidl_dynamic_type = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder(
+    rosidl_dynamic_type_builder);
+  if (!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_fini(rosidl_dynamic_type);
+      free(rosidl_dynamic_type);
+    });
+}
+
+
+DynamicType::DynamicType(
+  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_fini(rosidl_dynamic_type);
+      free(rosidl_dynamic_type);
+    });
+}
+
+
+DynamicType::DynamicType(
+  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!");
+    }
+  }
+}
+
+
+DynamicType::DynamicType(
+  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);
+}
+
+
+DynamicType::DynamicType(const DynamicType & other)
+: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_(nullptr)
+{
+  DynamicType out = other.clone();
+  std::swap(serialization_support_, out.serialization_support_);
+  std::swap(rosidl_dynamic_type_, out.rosidl_dynamic_type_);
+}
+
+
+DynamicType::DynamicType(DynamicType && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_(std::exchange(other.rosidl_dynamic_type_, nullptr)) {}
+
+
+DynamicType &
+DynamicType::operator=(const DynamicType & other)
+{
+  return *this = DynamicType(other);
+}
+
+
+DynamicType &
+DynamicType::operator=(DynamicType && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_);
+  return *this;
+}
+
+
+DynamicType::~DynamicType() {}
+
+
+void
+DynamicType::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription * description,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (!description) {
+    throw std::runtime_error("description cannot be nullptr!");
+  }
+  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;
+  rosidl_dynamic_type =
+    rosidl_dynamic_typesupport_dynamic_type_init_from_description(
+    serialization_support_->get_rosidl_serialization_support(), description);
+  if (!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_fini(rosidl_dynamic_type);
+      free(rosidl_dynamic_type);
+    });
+}
+
+
+bool
+DynamicType::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
+DynamicType::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_type_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicType::get_name() const
+{
+  size_t buf_length;
+  const char * buf = rosidl_dynamic_typesupport_dynamic_type_get_name(
+    get_rosidl_dynamic_type(), &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+size_t
+DynamicType::get_member_count() const
+{
+  return rosidl_dynamic_typesupport_dynamic_type_get_member_count(rosidl_dynamic_type_.get());
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_t *
+DynamicType::get_rosidl_dynamic_type()
+{
+  return rosidl_dynamic_type_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_t *
+DynamicType::get_rosidl_dynamic_type() const
+{
+  return rosidl_dynamic_type_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
+DynamicType::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>
+DynamicType::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
+DynamicType::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicType::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =========================================================================================
+DynamicType
+DynamicType::clone() const
+{
+  return DynamicType(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_type_clone(get_rosidl_dynamic_type()));
+}
+
+
+DynamicType::SharedPtr
+DynamicType::clone_shared() const
+{
+  return DynamicType::make_shared(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_type_clone(get_rosidl_dynamic_type()));
+}
+
+
+bool
+DynamicType::equals(const DynamicType & other) const
+{
+  if (get_library_identifier() != other.get_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  return rosidl_dynamic_typesupport_dynamic_type_equals(
+    get_rosidl_dynamic_type(), other.get_rosidl_dynamic_type());
+}
+
+
+DynamicData
+DynamicType::build_data()
+{
+  return DynamicData(shared_from_this());
+}
+
+
+DynamicData::SharedPtr
+DynamicType::build_data_shared()
+{
+  return DynamicData::make_shared(shared_from_this());
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_type_builder.cpp

@@ -0,0 +1,536 @@
+// 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 <memory>
+#include <string>
+
+#include "rclcpp/dynamic_typesupport/dynamic_data.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_type_builder.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rcutils/logging_macros.h"
+
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+#include <rosidl_dynamic_typesupport/types.h>
+
+using rclcpp::dynamic_typesupport::DynamicData;
+using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
+using rclcpp::dynamic_typesupport::DynamicType;
+using rclcpp::dynamic_typesupport::DynamicTypeBuilder;
+
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_TYPE_BUILDER_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_type_builder_impl.hpp"
+#endif
+
+
+// CONSTRUCTION ==================================================================================
+DynamicTypeBuilder::DynamicTypeBuilder(
+  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");
+  }
+}
+
+
+DynamicTypeBuilder::DynamicTypeBuilder(
+  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_fini(rosidl_dynamic_type_builder);
+      free(rosidl_dynamic_type_builder);
+    });
+}
+
+
+DynamicTypeBuilder::DynamicTypeBuilder(
+  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!");
+  }
+}
+
+
+DynamicTypeBuilder::DynamicTypeBuilder(
+  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);
+}
+
+
+DynamicTypeBuilder::DynamicTypeBuilder(const DynamicTypeBuilder & other)
+: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_builder_(nullptr)
+{
+  DynamicTypeBuilder out = other.clone();
+  std::swap(serialization_support_, out.serialization_support_);
+  std::swap(rosidl_dynamic_type_builder_, out.rosidl_dynamic_type_builder_);
+}
+
+
+DynamicTypeBuilder::DynamicTypeBuilder(DynamicTypeBuilder && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_builder_(std::exchange(other.rosidl_dynamic_type_builder_, nullptr)) {}
+
+
+DynamicTypeBuilder &
+DynamicTypeBuilder::operator=(const DynamicTypeBuilder & other)
+{
+  return *this = DynamicTypeBuilder(other);
+}
+
+
+DynamicTypeBuilder &
+DynamicTypeBuilder::operator=(DynamicTypeBuilder && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_);
+  return *this;
+}
+
+
+DynamicTypeBuilder::~DynamicTypeBuilder() {}
+
+
+void
+DynamicTypeBuilder::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription * description,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (!description) {
+    throw std::runtime_error("description cannot be nullptr!");
+  }
+  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;
+  rosidl_dynamic_type_builder =
+    rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description(
+    serialization_support_->get_rosidl_serialization_support(), description);
+  if (!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_fini(rosidl_dynamic_type_builder);
+      free(rosidl_dynamic_type_builder);
+    });
+}
+
+
+void
+DynamicTypeBuilder::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;
+  rosidl_dynamic_type_builder = rosidl_dynamic_typesupport_dynamic_type_builder_init(
+    serialization_support->get_rosidl_serialization_support(), name.c_str(), name.size());
+
+  if (!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_fini(rosidl_dynamic_type_builder);
+      free(rosidl_dynamic_type_builder);
+    });
+}
+
+
+bool
+DynamicTypeBuilder::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
+DynamicTypeBuilder::get_library_identifier() const
+{
+  return std::string(rosidl_dynamic_type_builder_->serialization_support->library_identifier);
+}
+
+
+const std::string
+DynamicTypeBuilder::get_name() const
+{
+  size_t buf_length;
+  const char * buf = rosidl_dynamic_typesupport_dynamic_type_builder_get_name(
+    get_rosidl_dynamic_type_builder(), &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_builder_t *
+DynamicTypeBuilder::get_rosidl_dynamic_type_builder()
+{
+  return rosidl_dynamic_type_builder_.get();
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_builder_t *
+DynamicTypeBuilder::get_rosidl_dynamic_type_builder() const
+{
+  return rosidl_dynamic_type_builder_.get();
+}
+
+
+std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>
+DynamicTypeBuilder::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>
+DynamicTypeBuilder::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
+DynamicTypeBuilder::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicTypeBuilder::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =======================================================================================
+void
+DynamicTypeBuilder::set_name(const std::string & name)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_set_name(
+    get_rosidl_dynamic_type_builder(), name.c_str(), name.size());
+}
+
+
+DynamicTypeBuilder
+DynamicTypeBuilder::clone() const
+{
+  return DynamicTypeBuilder(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_type_builder_clone(get_rosidl_dynamic_type_builder()));
+}
+
+
+DynamicTypeBuilder::SharedPtr
+DynamicTypeBuilder::clone_shared() const
+{
+  return DynamicTypeBuilder::make_shared(
+    serialization_support_,
+    rosidl_dynamic_typesupport_dynamic_type_builder_clone(get_rosidl_dynamic_type_builder()));
+}
+
+
+void
+DynamicTypeBuilder::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");
+  }
+}
+
+
+DynamicData
+DynamicTypeBuilder::build_data()
+{
+  return DynamicData(shared_from_this());
+}
+
+
+DynamicData::SharedPtr
+DynamicTypeBuilder::build_data_shared()
+{
+  return DynamicData::make_shared(shared_from_this());
+}
+
+
+DynamicType
+DynamicTypeBuilder::build_type()
+{
+  return DynamicType(shared_from_this());
+}
+
+
+DynamicType::SharedPtr
+DynamicTypeBuilder::build_type_shared()
+{
+  return DynamicType::make_shared(shared_from_this());
+}
+
+
+// ADD MEMBERS =====================================================================================
+// Defined in "detail/dynamic_type_builder_impl.hpp"
+
+
+// ADD BOUNDED STRING MEMBERS ======================================================================
+void
+DynamicTypeBuilder::add_bounded_string_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound);
+}
+
+
+void
+DynamicTypeBuilder::add_bounded_wstring_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound);
+}
+
+
+void
+DynamicTypeBuilder::add_bounded_string_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_bound, size_t array_length)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound, array_length);
+}
+
+
+void
+DynamicTypeBuilder::add_bounded_wstring_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_bound, size_t array_length)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound, array_length);
+}
+
+
+void
+DynamicTypeBuilder::add_bounded_string_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), string_bound);
+}
+
+
+void
+DynamicTypeBuilder::add_bounded_wstring_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(), wstring_bound);
+}
+
+
+void
+DynamicTypeBuilder::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)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    string_bound, sequence_bound);
+}
+
+
+void
+DynamicTypeBuilder::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)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    wstring_bound, sequence_bound);
+}
+
+
+// ADD NESTED MEMBERS ==============================================================================
+void
+DynamicTypeBuilder::add_complex_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicType & nested_type)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicTypeBuilder::add_complex_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicType & nested_type, size_t array_length)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type.get_rosidl_dynamic_type(), array_length);
+}
+
+
+void
+DynamicTypeBuilder::add_complex_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicType & nested_type)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicTypeBuilder::add_complex_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicType & nested_type, size_t sequence_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type.get_rosidl_dynamic_type(), sequence_bound);
+}
+
+
+void
+DynamicTypeBuilder::add_complex_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicTypeBuilder & nested_type_builder)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicTypeBuilder::add_complex_array_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicTypeBuilder & nested_type_builder, size_t array_length)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder(), array_length);
+}
+
+
+void
+DynamicTypeBuilder::add_complex_unbounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicTypeBuilder & nested_type_builder)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicTypeBuilder::add_complex_bounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicTypeBuilder & nested_type_builder, size_t sequence_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder(
+    get_rosidl_dynamic_type_builder(), id, name.c_str(), name.size(),
+    nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound);
+}

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,16 +37,14 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
   std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message)
 {}
 
-QOSEventHandlerBase::~QOSEventHandlerBase()
+EventHandlerBase::~EventHandlerBase()
 {
   // 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.
-  if (on_new_event_callback_) {
-    clear_on_ready_callback();
-  }
+  clear_on_ready_callback();
 
   if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
@@ -54,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) {
@@ -71,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)
 {
@@ -88,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

@@ -112,6 +112,12 @@ Executor::~Executor()
   }
   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);
+  }
+  weak_nodes_to_guard_conditions_.clear();
+
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
@@ -274,6 +280,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);
 }
 
@@ -378,6 +388,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);
 }
@@ -706,6 +719,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         auto weak_node_ptr = pair.second;
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
+          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
+          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
+            auto guard_condition = node_guard_pair->second;
+            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
         }
       }
       std::for_each(

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

rclcpp/src/rclcpp/publisher_base.cpp

@@ -37,7 +37,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/network_flow_endpoint.hpp"
 #include "rclcpp/node.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 
 using rclcpp::PublisherBase;
 
@@ -145,21 +145,48 @@ PublisherBase::bind_event_callbacks(
       event_callbacks.liveliness_callback,
       RCL_PUBLISHER_LIVELINESS_LOST);
   }
+
+  QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb;
   if (event_callbacks.incompatible_qos_callback) {
-    this->add_event_handler(
-      event_callbacks.incompatible_qos_callback,
-      RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    incompatible_qos_cb = event_callbacks.incompatible_qos_callback;
   } else if (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
+    incompatible_qos_cb = [this](QOSOfferedIncompatibleQoSInfo & info) {
+        this->default_incompatible_qos_callback(info);
+      };
+  }
+  try {
+    if (incompatible_qos_cb) {
+      this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
     }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible qos; wrong callback type");
+  }
+
+  IncompatibleTypeCallbackType incompatible_type_cb;
+  if (event_callbacks.incompatible_type_callback) {
+    incompatible_type_cb = event_callbacks.incompatible_type_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_type_cb = [this](IncompatibleTypeInfo & info) {
+        this->default_incompatible_type_callback(info);
+      };
+  }
+  try {
+    if (incompatible_type_cb) {
+      this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE);
+    }
+  } catch (UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible type; wrong callback type");
+  }
+  if (event_callbacks.matched_callback) {
+    this->add_event_handler(
+      event_callbacks.matched_callback,
+      RCL_PUBLISHER_MATCHED);
   }
 }
 
@@ -195,7 +222,7 @@ PublisherBase::get_publisher_handle() const
 }
 
 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>> &
 PublisherBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -265,7 +292,7 @@ PublisherBase::assert_liveliness() const
 bool
 PublisherBase::can_loan_messages() const
 {
-  return rcl_publisher_can_loan_messages(publisher_handle_.get());
+  return !intra_process_is_enabled_ && rcl_publisher_can_loan_messages(publisher_handle_.get());
 }
 
 bool
@@ -311,6 +338,17 @@ PublisherBase::default_incompatible_qos_callback(
     policy_name.c_str());
 }
 
+void
+PublisherBase::default_incompatible_type_callback(
+  rclcpp::IncompatibleTypeInfo & event) const
+{
+  (void)event;
+
+  RCLCPP_WARN(
+    rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())),
+    "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name());
+}
+
 std::vector<rclcpp::NetworkFlowEndpoint> PublisherBase::get_network_flow_endpoints() const
 {
   rcutils_allocator_t allocator = rcutils_get_default_allocator();

rclcpp/src/rclcpp/qos.cpp

@@ -45,9 +45,19 @@ std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind)
   }
 }
 
-QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg)
+QoSInitialization::QoSInitialization(
+  rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
+  bool print_depth_warning)
 : history_policy(history_policy_arg), depth(depth_arg)
-{}
+{
+  if (history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && depth == 0 && print_depth_warning) {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored. "
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
+}
 
 QoSInitialization
 QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos)
@@ -55,8 +65,9 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos)
   switch (rmw_qos.history) {
     case RMW_QOS_POLICY_HISTORY_KEEP_ALL:
       return KeepAll();
-    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
     case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
+      return KeepLast(rmw_qos.depth, false);
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
     case RMW_QOS_POLICY_HISTORY_UNKNOWN:
     default:
       return KeepLast(rmw_qos.depth);
@@ -67,16 +78,9 @@ KeepAll::KeepAll()
 : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0)
 {}
 
-KeepLast::KeepLast(size_t depth)
-: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth)
+KeepLast::KeepLast(size_t depth, bool print_depth_warning)
+: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth, print_depth_warning)
 {
-  if (depth == 0) {
-    RCLCPP_WARN_ONCE(
-      rclcpp::get_logger(
-        "rclcpp"),
-      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
-      "This will be interpreted as SYSTEM_DEFAULT");
-  }
 }
 
 QoS::QoS(
@@ -84,15 +88,6 @@ QoS::QoS(
   const rmw_qos_profile_t & initial_profile)
 : rmw_qos_profile_(initial_profile)
 {
-  if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST &&
-    qos_initialization.depth == 0)
-  {
-    RCLCPP_WARN_ONCE(
-      rclcpp::get_logger(
-        "rclcpp"),
-      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
-      "This will be interpreted as SYSTEM_DEFAULT");
-  }
   rmw_qos_profile_.history = qos_initialization.history_policy;
   rmw_qos_profile_.depth = qos_initialization.depth;
 }

rclcpp/src/rclcpp/service.cpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -131,7 +132,7 @@ ServiceBase::set_on_new_request_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 request callback for service");
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to set the on new request callback for service");
   }
 }

rclcpp/src/rclcpp/subscription_base.cpp

@@ -27,7 +27,7 @@
 #include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
-#include "rclcpp/qos_event.hpp"
+#include "rclcpp/event_handler.hpp"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -114,32 +114,58 @@ SubscriptionBase::bind_event_callbacks(
       event_callbacks.deadline_callback,
       RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
   }
+
   if (event_callbacks.liveliness_callback) {
     this->add_event_handler(
       event_callbacks.liveliness_callback,
       RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
   }
+
+  QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb;
   if (event_callbacks.incompatible_qos_callback) {
-    this->add_event_handler(
-      event_callbacks.incompatible_qos_callback,
-      RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    incompatible_qos_cb = event_callbacks.incompatible_qos_callback;
   } else if (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
-    }
+    incompatible_qos_cb = [this](QOSRequestedIncompatibleQoSInfo & info) {
+        this->default_incompatible_qos_callback(info);
+      };
   }
+  // Register default callback when not specified
+  try {
+    if (incompatible_qos_cb) {
+      this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    // pass
+  }
+
+  IncompatibleTypeCallbackType incompatible_type_cb;
+  if (event_callbacks.incompatible_type_callback) {
+    incompatible_type_cb = event_callbacks.incompatible_type_callback;
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    incompatible_type_cb = [this](IncompatibleTypeInfo & info) {
+        this->default_incompatible_type_callback(info);
+      };
+  }
+  try {
+    if (incompatible_type_cb) {
+      this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE);
+    }
+  } catch (UnsupportedEventTypeException & /*exc*/) {
+    // pass
+  }
+
   if (event_callbacks.message_lost_callback) {
     this->add_event_handler(
       event_callbacks.message_lost_callback,
       RCL_SUBSCRIPTION_MESSAGE_LOST);
   }
+  if (event_callbacks.matched_callback) {
+    this->add_event_handler(
+      event_callbacks.matched_callback,
+      RCL_SUBSCRIPTION_MATCHED);
+  }
 }
 
 const char *
@@ -161,7 +187,7 @@ SubscriptionBase::get_subscription_handle() const
 }
 
 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>> &
 SubscriptionBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -299,6 +325,17 @@ SubscriptionBase::default_incompatible_qos_callback(
     policy_name.c_str());
 }
 
+void
+SubscriptionBase::default_incompatible_type_callback(
+  rclcpp::IncompatibleTypeInfo & event) const
+{
+  (void)event;
+
+  RCLCPP_WARN(
+    rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())),
+    "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name());
+}
+
 bool
 SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const
 {

rclcpp/src/rclcpp/time_source.cpp

@@ -37,7 +37,8 @@ class ClocksState final
 {
 public:
   ClocksState()
-  : logger_(rclcpp::get_logger("rclcpp"))
+  : logger_(rclcpp::get_logger("rclcpp")),
+    last_time_msg_(std::make_shared<builtin_interfaces::msg::Time>())
   {
   }
 
@@ -53,13 +54,8 @@ public:
     ros_time_active_ = true;
 
     // Update all attached clocks to zero or last recorded time
-    std::lock_guard<std::mutex> guard(clock_list_lock_);
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
-    if (last_msg_set_) {
-      time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    }
     for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-      set_clock(time_msg, true, *it);
+      set_clock(last_time_msg_, true, *it);
     }
   }
 
@@ -101,11 +97,7 @@ public:
     std::lock_guard<std::mutex> guard(clock_list_lock_);
     associated_clocks_.push_back(clock);
     // Set the clock to zero unless there's a recently received message
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
-    if (last_msg_set_) {
-      time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    }
-    set_clock(time_msg, ros_time_active_, clock);
+    set_clock(last_time_msg_, ros_time_active_, clock);
   }
 
   // Detach a clock
@@ -171,7 +163,7 @@ public:
   // Cache the last clock message received
   void cache_last_msg(std::shared_ptr<const rosgraph_msgs::msg::Clock> msg)
   {
-    last_msg_set_ = msg;
+    last_time_msg_ = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
   }
 
   bool are_all_clocks_rcl_ros_time()
@@ -199,7 +191,7 @@ private:
   // This is needed when new clocks are added.
   bool ros_time_active_{false};
   // Last set message to be passed to newly registered clocks
-  std::shared_ptr<const rosgraph_msgs::msg::Clock> last_msg_set_;
+  std::shared_ptr<builtin_interfaces::msg::Time> last_time_msg_{nullptr};
 };
 
 class TimeSource::NodeState final

rclcpp/src/rclcpp/timer.cpp

@@ -19,9 +19,12 @@
 #include <memory>
 #include <thread>
 
-#include "rclcpp/contexts/default_context.hpp"
-#include "rclcpp/exceptions.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
 
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/detail/cpp_callback_trampoline.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 #include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
@@ -71,7 +74,9 @@ TimerBase::TimerBase(
 }
 
 TimerBase::~TimerBase()
-{}
+{
+  clear_on_reset_callback();
+}
 
 void
 TimerBase::cancel()
@@ -96,7 +101,11 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
+  rcl_ret_t ret = RCL_RET_OK;
+  {
+    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    ret = rcl_timer_reset(timer_handle_.get());
+  }
   if (ret != RCL_RET_OK) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
   }
@@ -138,3 +147,73 @@ TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+TimerBase::set_on_reset_callback(std::function<void(size_t)> callback)
+{
+  if (!callback) {
+    throw std::invalid_argument(
+            "The callback passed to set_on_reset_callback "
+            "is not callable.");
+  }
+
+  auto new_callback =
+    [callback, this](size_t reset_calls) {
+      try {
+        callback(reset_calls);
+      } catch (const std::exception & exception) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("rclcpp"),
+          "rclcpp::TimerBase@" << this <<
+            " caught " << rmw::impl::cpp::demangle(exception) <<
+            " exception in user-provided callback for the 'on reset' callback: " <<
+            exception.what());
+      } catch (...) {
+        RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger("rclcpp"),
+          "rclcpp::TimerBase@" << this <<
+            " caught unhandled exception in user-provided callback " <<
+            "for the 'on reset' 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 rcl hasn't been told about the new one yet.
+  set_on_reset_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_reset_callback_ = new_callback;
+
+  // Set it again, now using the permanent storage.
+  set_on_reset_callback(
+    rclcpp::detail::cpp_callback_trampoline<
+      decltype(on_reset_callback_), const void *, size_t>,
+    static_cast<const void *>(&on_reset_callback_));
+}
+
+void
+TimerBase::clear_on_reset_callback()
+{
+  std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+
+  if (on_reset_callback_) {
+    set_on_reset_callback(nullptr, nullptr);
+    on_reset_callback_ = nullptr;
+  }
+}
+
+void
+TimerBase::set_on_reset_callback(rcl_event_callback_t callback, const void * user_data)
+{
+  rcl_ret_t ret = rcl_timer_set_on_reset_callback(timer_handle_.get(), callback, user_data);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer on reset callback");
+  }
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -444,17 +444,23 @@ function(test_generic_pubsub_for_rmw_implementation)
   endif()
 endfunction()
 call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation)
-ament_add_gtest(test_qos_event test_qos_event.cpp)
-if(TARGET test_qos_event)
-  ament_target_dependencies(test_qos_event
-    "rmw"
-    "test_msgs"
+
+function(test_qos_event_for_rmw_implementation)
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+  ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp
+    ENV ${rmw_implementation_env_var}
   )
-  target_link_libraries(test_qos_event
-    ${PROJECT_NAME}
-    mimick
-  )
-endif()
+  if(TARGET test_qos_event${target_suffix})
+    target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick)
+    ament_target_dependencies(test_qos_event${target_suffix}
+      "rmw"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+  endif()
+endfunction()
+call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation)
+
 ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp)
 if(TARGET test_qos_overriding_options)
   target_link_libraries(test_qos_overriding_options
@@ -508,6 +514,17 @@ if(TARGET test_service)
   )
   target_link_libraries(test_service ${PROJECT_NAME} mimick)
 endif()
+ament_add_gmock(test_service_introspection test_service_introspection.cpp)
+if(TARGET test_service)
+  ament_target_dependencies(test_service_introspection
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick)
+endif()
 # Creating and destroying nodes is slow with Connext, so this needs larger timeout.
 ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
 if(TARGET test_subscription)
@@ -582,6 +599,9 @@ target_link_libraries(test_logger ${PROJECT_NAME})
 ament_add_gmock(test_logging test_logging.cpp)
 target_link_libraries(test_logging ${PROJECT_NAME})
 
+ament_add_gmock(test_context test_context.cpp)
+target_link_libraries(test_context ${PROJECT_NAME})
+
 ament_add_gtest(test_time test_time.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_time)
@@ -736,6 +756,12 @@ if(TARGET test_rosout_qos)
   target_link_libraries(test_rosout_qos ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp)
+if(TARGET test_rosout_subscription)
+  ament_target_dependencies(test_rosout_subscription "rcl")
+  target_link_libraries(test_rosout_subscription ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_executor test_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 120)

rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp

@@ -33,6 +33,15 @@ protected:
   }
 };
 
+TEST_F(TestNodeInterfaces, default_constructor) {
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  NodeInterfaces<NodeBaseInterface, NodeGraphInterface> interfaces;
+  interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(*node);
+}
+
 /*
    Testing NodeInterfaces construction from nodes.
  */
@@ -44,7 +53,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
     using rclcpp::node_interfaces::NodeInterfaces;
     using rclcpp::node_interfaces::NodeBaseInterface;
     using rclcpp::node_interfaces::NodeGraphInterface;
-    auto node_interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(node);
+    auto node_interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(*node);
   }
 
   // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base.
@@ -55,7 +64,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
         auto base_interface = ni.get<NodeBaseInterface>();
       };
 
-    some_func(node);
+    some_func(*node);
   }
 
   // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces.
@@ -67,7 +76,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
         auto base_interface = ni_with_one.get<NodeBaseInterface>();
       };
 
-    NodeInterfaces<NodeBaseInterface, NodeGraphInterface> ni_with_two(node);
+    NodeInterfaces<NodeBaseInterface, NodeGraphInterface> ni_with_two(*node);
 
     some_func(ni_with_two);
   }
@@ -102,7 +111,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) {
     rclcpp::node_interfaces::NodeWaitablesInterface,
     rclcpp::node_interfaces::NodeParametersInterface,
     rclcpp::node_interfaces::NodeTimeSourceInterface
-    >(node);
+    >(*node);
 }
 
 /*
@@ -134,7 +143,7 @@ TEST_F(TestNodeInterfaces, ni_init) {
     NodeWaitablesInterface,
     NodeParametersInterface,
     NodeTimeSourceInterface
-    >(node);
+    >(*node);
 
   {
     auto base = ni.get<NodeBaseInterface>();
@@ -198,7 +207,7 @@ TEST_F(TestNodeInterfaces, ni_all_init) {
   using rclcpp::node_interfaces::NodeParametersInterface;
   using rclcpp::node_interfaces::NodeTimeSourceInterface;
 
-  auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(node);
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(*node);
 
   {
     auto base = ni.get<NodeBaseInterface>();

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -63,6 +63,57 @@ public:
   }
 };
 
+static bool test_waitable_result2 = false;
+
+class TestWaitable2 : public rclcpp::Waitable
+{
+public:
+  explicit TestWaitable2(rcl_publisher_t * pub_ptr)
+  : pub_ptr_(pub_ptr),
+    pub_event_(rcl_get_zero_initialized_event())
+  {
+    EXPECT_EQ(
+      rcl_publisher_event_init(&pub_event_, pub_ptr_, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED),
+      RCL_RET_OK);
+  }
+
+  ~TestWaitable2()
+  {
+    EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK);
+  }
+
+  void add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    EXPECT_EQ(rcl_wait_set_add_event(wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK);
+  }
+
+  bool is_ready(rcl_wait_set_t *) override
+  {
+    return test_waitable_result2;
+  }
+
+  std::shared_ptr<void>
+  take_data() override
+  {
+    return nullptr;
+  }
+
+  void execute(std::shared_ptr<void> & data) override
+  {
+    (void) data;
+  }
+
+  size_t get_number_of_ready_events() override
+  {
+    return 1;
+  }
+
+private:
+  rcl_publisher_t * pub_ptr_;
+  rcl_event_t pub_event_;
+  size_t wait_set_event_index_;
+};
+
 struct RclWaitSetSizes
 {
   size_t size_of_subscriptions = 0;
@@ -497,8 +548,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) {
 TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) {
   RclWaitSetSizes expected_sizes = {};
   expected_sizes.size_of_subscriptions = 1;
-  expected_sizes.size_of_events = 1;
-  expected_sizes.size_of_waitables = 1;
+  expected_sizes.size_of_events = 2;
+  expected_sizes.size_of_waitables = 2;
   auto node_with_subscription = create_node_with_subscription("subscription_node");
   EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes));
 }
@@ -657,20 +708,129 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer) {
 }
 
 TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) {
-  auto node1 = std::make_shared<rclcpp::Node>("waitable_node", "ns");
-  auto node2 = std::make_shared<rclcpp::Node>("waitable_node2", "ns");
-  rclcpp::Waitable::SharedPtr waitable1 = std::make_shared<TestWaitable>();
-  rclcpp::Waitable::SharedPtr waitable2 = std::make_shared<TestWaitable>();
-  node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
-  node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
-
   auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) {
       rclcpp::AnyExecutable result;
       allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes);
       return result;
     };
 
-  EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity));
+  {
+    auto node1 = std::make_shared<rclcpp::Node>(
+      "waitable_node", "ns",
+      rclcpp::NodeOptions()
+      .start_parameter_event_publisher(false)
+      .start_parameter_services(false));
+
+    rclcpp::PublisherOptions pub_options;
+    pub_options.use_default_callbacks = false;
+
+    auto pub1 = node1->create_publisher<test_msgs::msg::Empty>(
+      "test_topic_1", rclcpp::QoS(10), pub_options);
+
+    auto waitable1 =
+      std::make_shared<TestWaitable2>(pub1->get_publisher_handle().get());
+    node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr);
+
+    auto basic_node = create_node_with_disabled_callback_groups("basic_node");
+    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
+    basic_node->for_each_callback_group(
+      [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            group_ptr,
+            basic_node->get_node_base_interface()));
+      });
+    node1->for_each_callback_group(
+      [node1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
+        weak_groups_to_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            group_ptr,
+            node1->get_node_base_interface()));
+      });
+    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
+
+    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+    ASSERT_EQ(
+      rcl_wait_set_init(
+        &wait_set,
+        allocator_memory_strategy()->number_of_ready_subscriptions(),
+        allocator_memory_strategy()->number_of_guard_conditions(),
+        allocator_memory_strategy()->number_of_ready_timers(),
+        allocator_memory_strategy()->number_of_ready_clients(),
+        allocator_memory_strategy()->number_of_ready_services(),
+        allocator_memory_strategy()->number_of_ready_events(),
+        rclcpp::contexts::get_global_default_context()->get_rcl_context().get(),
+        allocator_memory_strategy()->get_allocator()),
+      RCL_RET_OK);
+
+    ASSERT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(&wait_set));
+
+    ASSERT_EQ(
+      rcl_wait(
+        &wait_set,
+        std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::milliseconds(100))
+        .count()),
+      RCL_RET_OK);
+    test_waitable_result2 = true;
+    allocator_memory_strategy()->remove_null_handles(&wait_set);
+
+    rclcpp::AnyExecutable result = get_next_entity(weak_groups_to_nodes);
+    EXPECT_EQ(result.node_base, node1->get_node_base_interface());
+    test_waitable_result2 = false;
+
+    EXPECT_EQ(rcl_wait_set_fini(&wait_set), RCL_RET_OK);
+  }
+
+  {
+    auto node2 = std::make_shared<rclcpp::Node>(
+      "waitable_node2", "ns",
+      rclcpp::NodeOptions()
+      .start_parameter_services(false)
+      .start_parameter_event_publisher(false));
+
+    rclcpp::PublisherOptions pub_options;
+    pub_options.use_default_callbacks = false;
+
+    auto pub2 = node2->create_publisher<test_msgs::msg::Empty>(
+      "test_topic_2", rclcpp::QoS(10), pub_options);
+
+    auto waitable2 =
+      std::make_shared<TestWaitable2>(pub2->get_publisher_handle().get());
+    node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr);
+
+    auto basic_node2 = std::make_shared<rclcpp::Node>(
+      "basic_node2", "ns",
+      rclcpp::NodeOptions()
+      .start_parameter_services(false)
+      .start_parameter_event_publisher(false));
+    WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes;
+    basic_node2->for_each_callback_group(
+      [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
+        weak_groups_to_uncollected_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            group_ptr,
+            basic_node2->get_node_base_interface()));
+      });
+    node2->for_each_callback_group(
+      [node2,
+      &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
+        weak_groups_to_uncollected_nodes.insert(
+          std::pair<rclcpp::CallbackGroup::WeakPtr,
+          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
+            group_ptr,
+            node2->get_node_base_interface()));
+      });
+
+    rclcpp::AnyExecutable failed_result = get_next_entity(weak_groups_to_uncollected_nodes);
+    EXPECT_EQ(failed_result.node_base, nullptr);
+  }
 }
 
 TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) {

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -340,6 +340,53 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess
   EXPECT_TRUE(received_message_future.get());
 }
 
+/*
+ * Test callback group created after spin.
+ * A subscriber with a new callback group that created after executor spin not received a message
+ * because the executor can't be triggered while a subscriber created, see
+ * https://github.com/ros2/rclcpp/issues/2067
+*/
+TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin)
+{
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  // create a publisher to send data
+  rclcpp::QoS qos = rclcpp::QoS(1).reliable().transient_local();
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher =
+    node->create_publisher<test_msgs::msg::Empty>("topic_name", qos);
+  publisher->publish(test_msgs::msg::Empty());
+
+  // create a thread running an executor
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::promise<bool> received_message_promise;
+  auto received_message_future = received_message_promise.get_future();
+  rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT;
+  std::thread executor_thread = std::thread(
+    [&executor, &received_message_future, &return_code]() {
+      return_code = executor.spin_until_future_complete(received_message_future, 5s);
+    });
+
+  // to create a callback group after spin
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  // expect the subscriber to receive a message
+  auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) {
+      received_message_promise.set_value(true);
+    };
+  // create a subscription using the `cb_grp` callback group
+  auto options = rclcpp::SubscriptionOptions();
+  options.callback_group = cb_grp;
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription =
+    node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, sub_callback, options);
+
+  executor_thread.join();
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(received_message_future.get());
+}
+
 /*
  * Test removing callback group from executor that its not associated with.
  */

rclcpp/test/rclcpp/test_context.cpp

@@ -0,0 +1,216 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// 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 <gtest/gtest.h>
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+TEST(TestContext, check_pre_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_pre_shutdown_callback(callback1);
+  context->add_pre_shutdown_callback(callback2);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_pre_shutdown_callback(callback4);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4);
+}
+
+TEST(TestContext, check_on_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_on_shutdown_callback(callback1);
+  context->add_on_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback4);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4);
+}
+
+TEST(TestContext, check_mixed_register_shutdown_callback_order) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+  auto callback5 = [&result, &index]() {
+      result[index] = 5;
+      index++;
+    };
+  auto callback6 = [&result, &index]() {
+      result[index] = 6;
+      index++;
+    };
+  auto callback7 = [&result, &index]() {
+      result[index] = 7;
+      index++;
+    };
+  auto callback8 = [&result, &index]() {
+      result[index] = 8;
+      index++;
+    };
+
+  // Mixed register
+  context->add_pre_shutdown_callback(callback1);
+  context->add_on_shutdown_callback(callback5);
+  context->add_pre_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback6);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback7);
+  context->add_pre_shutdown_callback(callback4);
+  context->add_on_shutdown_callback(callback8);
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(
+    result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4 &&
+    result[4] == 5 && result[5] == 6 && result[6] == 7 && result[7] == 8);
+}
+
+TEST(TestContext, check_pre_shutdown_callback_order_after_del) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_pre_shutdown_callback(callback1);
+  auto callback_handle = context->add_pre_shutdown_callback(callback2);
+  context->add_pre_shutdown_callback(callback3);
+  context->add_pre_shutdown_callback(callback4);
+
+  EXPECT_TRUE(context->remove_pre_shutdown_callback(callback_handle));
+  EXPECT_FALSE(context->remove_pre_shutdown_callback(callback_handle));
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0);
+}
+
+TEST(TestContext, check_on_shutdown_callback_order_after_del) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  int result[4] = {0, 0, 0, 0};
+  int index = 0;
+
+  auto callback1 = [&result, &index]() {
+      result[index] = 1;
+      index++;
+    };
+  auto callback2 = [&result, &index]() {
+      result[index] = 2;
+      index++;
+    };
+  auto callback3 = [&result, &index]() {
+      result[index] = 3;
+      index++;
+    };
+  auto callback4 = [&result, &index]() {
+      result[index] = 4;
+      index++;
+    };
+
+  context->add_on_shutdown_callback(callback1);
+  auto callback_handle = context->add_on_shutdown_callback(callback2);
+  context->add_on_shutdown_callback(callback3);
+  context->add_on_shutdown_callback(callback4);
+
+  EXPECT_TRUE(context->remove_on_shutdown_callback(callback_handle));
+  EXPECT_FALSE(context->remove_on_shutdown_callback(callback_handle));
+
+  context->shutdown("for test");
+
+  EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0);
+}

rclcpp/test/rclcpp/test_function_traits.cpp

@@ -393,6 +393,7 @@ TEST(TestFunctionTraits, argument_types) {
 
   auto bind_one_bool = std::bind(
     &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1);
+  (void)bind_one_bool;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -402,6 +403,7 @@ TEST(TestFunctionTraits, argument_types) {
 
   auto bind_one_bool_const = std::bind(
     &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1);
+  (void)bind_one_bool_const;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -413,6 +415,7 @@ TEST(TestFunctionTraits, argument_types) {
   auto bind_two_bools = std::bind(
     &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1,
     std::placeholders::_2);
+  (void)bind_two_bools;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -429,6 +432,7 @@ TEST(TestFunctionTraits, argument_types) {
   auto bind_one_bool_one_float = std::bind(
     &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1,
     std::placeholders::_2);
+  (void)bind_one_bool_one_float;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -447,6 +451,7 @@ TEST(TestFunctionTraits, argument_types) {
     >::value, "Functor accepts a float as second argument");
 
   auto bind_one_int = std::bind(func_one_int, std::placeholders::_1);
+  (void)bind_one_int;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -455,6 +460,7 @@ TEST(TestFunctionTraits, argument_types) {
     >::value, "Functor accepts an int as first argument");
 
   auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2);
+  (void)bind_two_ints;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -470,6 +476,7 @@ TEST(TestFunctionTraits, argument_types) {
 
   auto bind_one_int_one_char = std::bind(
     func_one_int_one_char, std::placeholders::_1, std::placeholders::_2);
+  (void)bind_one_int_one_char;  // to quiet clang
 
   static_assert(
     std::is_same<
@@ -530,18 +537,21 @@ TEST(TestFunctionTraits, check_arguments) {
       (void)one;
       return 1;
     };
+  (void)lambda_one_int;  // to quiet clang
 
   auto lambda_two_ints = [](int one, int two) {
       (void)one;
       (void)two;
       return 2;
     };
+  (void)lambda_two_ints;  // to quiet clang
 
   auto lambda_one_int_one_char = [](int one, char two) {
       (void)one;
       (void)two;
       return 3;
     };
+  (void)lambda_one_int_one_char;  // to quiet clang
 
   static_assert(
     rclcpp::function_traits::check_arguments<decltype(lambda_one_int), int>::value,
@@ -572,6 +582,7 @@ TEST(TestFunctionTraits, check_arguments) {
 
   auto bind_one_bool = std::bind(
     &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1);
+  (void)bind_one_bool;  // to quiet clang
 
   // Test std::bind functions
   static_assert(
@@ -580,6 +591,7 @@ TEST(TestFunctionTraits, check_arguments) {
 
   auto bind_one_bool_const = std::bind(
     &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1);
+  (void)bind_one_bool_const;  // to quiet clang
 
   // Test std::bind functions
   static_assert(
@@ -745,6 +757,7 @@ TEST_F(TestMember, bind_member_functor) {
   auto bind_member_functor = std::bind(
     &TestMember::MemberFunctor, this, std::placeholders::_1,
     std::placeholders::_2, std::placeholders::_3);
+  (void)bind_member_functor;  // to quiet clang
 
   static_assert(
     rclcpp::function_traits::check_arguments<decltype(bind_member_functor), int, float,

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -186,7 +186,6 @@ TEST_F(TestLoanedMessage, move_loaned_message) {
   auto loaned_msg_moved_to = LoanedMessageT(std::move(loaned_msg_to_move));
 
   ASSERT_TRUE(loaned_msg_moved_to.is_valid());
-  ASSERT_FALSE(loaned_msg_to_move.is_valid());
 
   loaned_msg_moved_to.get().float32_value = 42.0f;
   ASSERT_EQ(42.0f, loaned_msg_moved_to.get().float32_value);

rclcpp/test/rclcpp/test_publisher.cpp

@@ -409,9 +409,7 @@ TEST_F(TestPublisher, intra_process_publish_failures) {
   std::allocator<void> allocator;
   {
     rclcpp::LoanedMessage<test_msgs::msg::Empty> loaned_msg(*publisher, allocator);
-    RCLCPP_EXPECT_THROW_EQ(
-      publisher->publish(std::move(loaned_msg)),
-      std::runtime_error("storing loaned messages in intra process is not supported yet"));
+    EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
   }
 
   {

rclcpp/test/rclcpp/test_qos_event.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <atomic>
 #include <chrono>
 #include <functional>
 #include <future>
@@ -232,7 +233,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) {
     auto throwing_statement = [callback, rcl_handle, event_type]() {
         // reset() is not needed for the exception, but it handles unused return value warning
         std::make_shared<
-          rclcpp::QOSEventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
+          rclcpp::EventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
           callback, rcl_publisher_event_init, rcl_handle, event_type).reset();
       };
     // This is done through a lambda because the compiler is having trouble parsing the templated
@@ -248,7 +249,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) {
     auto throwing_statement = [callback, rcl_handle, event_type]() {
         // reset() is needed for this exception
         std::make_shared<
-          rclcpp::QOSEventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
+          rclcpp::EventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
           callback, rcl_publisher_event_init, rcl_handle, event_type).reset();
       };
 
@@ -267,7 +268,7 @@ TEST_F(TestQosEvent, execute) {
   auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;};
   rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED;
 
-  rclcpp::QOSEventHandler<decltype(callback), decltype(rcl_handle)> handler(
+  rclcpp::EventHandler<decltype(callback), decltype(rcl_handle)> handler(
     callback, rcl_publisher_event_init, rcl_handle, event_type);
 
   std::shared_ptr<void> data = handler.take_data();
@@ -292,7 +293,7 @@ TEST_F(TestQosEvent, add_to_wait_set) {
   auto callback = [](int) {};
 
   rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED;
-  rclcpp::QOSEventHandler<decltype(callback), decltype(rcl_handle)> handler(
+  rclcpp::EventHandler<decltype(callback), decltype(rcl_handle)> handler(
     callback, rcl_publisher_event_init, rcl_handle, event_type);
 
   EXPECT_EQ(1u, handler.get_number_of_ready_events());
@@ -313,6 +314,11 @@ TEST_F(TestQosEvent, add_to_wait_set) {
 
 TEST_F(TestQosEvent, test_on_new_event_callback)
 {
+  // rmw_connextdds doesn't support rmw_event_set_callback() interface
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
   auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1));
   auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2));
 
@@ -354,6 +360,11 @@ TEST_F(TestQosEvent, test_on_new_event_callback)
 
 TEST_F(TestQosEvent, test_invalid_on_new_event_callback)
 {
+  // rmw_connextdds doesn't support rmw_event_set_callback() interface
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
   auto pub = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
   auto sub = node->create_subscription<test_msgs::msg::Empty>(topic_name, 10, message_callback);
   auto dummy_cb = [](size_t count_events) {(void)count_events;};
@@ -376,6 +387,12 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback)
   EXPECT_NO_THROW(
     pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS));
 
+  EXPECT_NO_THROW(
+    pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_MATCHED));
+
+  EXPECT_NO_THROW(
+    pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_MATCHED));
+
   EXPECT_NO_THROW(
     sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED));
 
@@ -394,6 +411,12 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback)
   EXPECT_NO_THROW(
     sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS));
 
+  EXPECT_NO_THROW(
+    sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_MATCHED));
+
+  EXPECT_NO_THROW(
+    sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_MATCHED));
+
   std::function<void(size_t)> invalid_cb;
 
   rclcpp::SubscriptionOptions sub_options;
@@ -413,3 +436,170 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback)
     pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED),
     std::invalid_argument);
 }
+
+TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback)
+{
+  // rmw_connextdds doesn't support rmw_event_set_callback() interface
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
+  std::atomic_size_t matched_count = 0;
+
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback = [](auto) {};
+  auto pub = node->create_publisher<test_msgs::msg::Empty>(
+    topic_name, 10, pub_options);
+
+  auto matched_event_callback = [&matched_count](size_t count) {
+      matched_count += count;
+    };
+
+  pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED);
+
+  rclcpp::executors::SingleThreadedExecutor ex;
+  ex.add_node(node->get_node_base_interface());
+
+  const auto timeout = std::chrono::milliseconds(200);
+
+  {
+    auto sub1 = node->create_subscription<test_msgs::msg::Empty>(topic_name, 10, message_callback);
+    ex.spin_some(timeout);
+    EXPECT_EQ(matched_count, static_cast<size_t>(1));
+
+    {
+      auto sub2 = node->create_subscription<test_msgs::msg::Empty>(
+        topic_name, 10, message_callback);
+      ex.spin_some(timeout);
+      EXPECT_EQ(matched_count, static_cast<size_t>(2));
+    }
+    ex.spin_some(timeout);
+    EXPECT_EQ(matched_count, static_cast<size_t>(3));
+  }
+
+  ex.spin_some(timeout);
+  EXPECT_EQ(matched_count, static_cast<size_t>(4));
+}
+
+TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback)
+{
+  // rmw_connextdds doesn't support rmw_event_set_callback() interface
+  if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) {
+    GTEST_SKIP();
+  }
+
+  std::atomic_size_t matched_count = 0;
+
+  rclcpp::SubscriptionOptions sub_options;
+  sub_options.event_callbacks.matched_callback = [](auto) {};
+  auto sub = node->create_subscription<test_msgs::msg::Empty>(
+    topic_name, 10, message_callback, sub_options);
+
+  auto matched_event_callback = [&matched_count](size_t count) {
+      matched_count += count;
+    };
+
+  sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED);
+
+  rclcpp::executors::SingleThreadedExecutor ex;
+  ex.add_node(node->get_node_base_interface());
+
+  const auto timeout = std::chrono::milliseconds(200);
+
+  {
+    auto pub1 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+
+    ex.spin_some(timeout);
+    EXPECT_EQ(matched_count, static_cast<size_t>(1));
+
+    {
+      auto pub2 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+      ex.spin_some(timeout);
+      EXPECT_EQ(matched_count, static_cast<size_t>(2));
+    }
+
+    ex.spin_some(timeout);
+    EXPECT_EQ(matched_count, static_cast<size_t>(3));
+  }
+
+  ex.spin_some(timeout);
+  EXPECT_EQ(matched_count, static_cast<size_t>(4));
+}
+
+TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback)
+{
+  rmw_matched_status_t matched_expected_result;
+
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [&matched_expected_result](rmw_matched_status_t & s) {
+      EXPECT_EQ(s.total_count, matched_expected_result.total_count);
+      EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change);
+      EXPECT_EQ(s.current_count, matched_expected_result.current_count);
+      EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change);
+    };
+
+  auto pub = node->create_publisher<test_msgs::msg::Empty>(
+    topic_name, 10, pub_options);
+
+  rclcpp::executors::SingleThreadedExecutor ex;
+  ex.add_node(node->get_node_base_interface());
+
+  // Create a connected subscription
+  matched_expected_result.total_count = 1;
+  matched_expected_result.total_count_change = 1;
+  matched_expected_result.current_count = 1;
+  matched_expected_result.current_count_change = 1;
+
+  const auto timeout = std::chrono::milliseconds(200);
+
+  {
+    auto sub = node->create_subscription<test_msgs::msg::Empty>(topic_name, 10, message_callback);
+    ex.spin_some(timeout);
+
+    // destroy a connected subscription
+    matched_expected_result.total_count = 1;
+    matched_expected_result.total_count_change = 0;
+    matched_expected_result.current_count = 0;
+    matched_expected_result.current_count_change = -1;
+  }
+  ex.spin_some(timeout);
+}
+
+TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback)
+{
+  rmw_matched_status_t matched_expected_result;
+
+  rclcpp::SubscriptionOptions sub_options;
+  sub_options.event_callbacks.matched_callback =
+    [&matched_expected_result](rmw_matched_status_t & s) {
+      EXPECT_EQ(s.total_count, matched_expected_result.total_count);
+      EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change);
+      EXPECT_EQ(s.current_count, matched_expected_result.current_count);
+      EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change);
+    };
+  auto sub = node->create_subscription<test_msgs::msg::Empty>(
+    topic_name, 10, message_callback, sub_options);
+
+  rclcpp::executors::SingleThreadedExecutor ex;
+  ex.add_node(node->get_node_base_interface());
+
+  // Create a connected publisher
+  matched_expected_result.total_count = 1;
+  matched_expected_result.total_count_change = 1;
+  matched_expected_result.current_count = 1;
+  matched_expected_result.current_count_change = 1;
+
+  const auto timeout = std::chrono::milliseconds(200);
+  {
+    auto pub1 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+    ex.spin_some(timeout);
+
+    // destroy a connected publisher
+    matched_expected_result.total_count = 1;
+    matched_expected_result.total_count_change = 0;
+    matched_expected_result.current_count = 0;
+    matched_expected_result.current_count_change = -1;
+  }
+  ex.spin_some(timeout);
+}

rclcpp/test/rclcpp/test_rosout_subscription.cpp

@@ -0,0 +1,180 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/msg/log.hpp"
+
+using namespace std::chrono_literals;
+
+class TestRosoutSubscription : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("test_rosout_subscription", "/ns");
+    sub = node->create_subscription<rcl_interfaces::msg::Log>(
+      "/rosout", 10, [this](rcl_interfaces::msg::Log::ConstSharedPtr msg) {
+        if (msg->msg == this->rosout_msg_data &&
+        msg->name == this->rosout_msg_name)
+        {
+          received_msg_promise.set_value(true);
+        }
+      });
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Subscription<rcl_interfaces::msg::Log>::SharedPtr sub;
+  std::promise<bool> received_msg_promise;
+  std::string rosout_msg_data;
+  std::string rosout_msg_name;
+};
+
+TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) {
+  std::string logger_name = "ns.test_rosout_subscription.child";
+  this->rosout_msg_data = "SOMETHING";
+  this->rosout_msg_name = logger_name;
+  {
+    // before calling get_child of Logger
+    {
+      RCLCPP_INFO(
+        rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str());
+      auto future = received_msg_promise.get_future();
+      auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+      ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code);
+      received_msg_promise = {};
+    }
+
+    rclcpp::Logger child_logger = this->node->get_logger().get_child("child");
+    ASSERT_EQ(child_logger.get_name(), logger_name);
+
+    // after calling get_child of Logger
+    // 1. use child_logger directly
+    {
+      RCLCPP_INFO(child_logger, this->rosout_msg_data.c_str());
+      auto future = received_msg_promise.get_future();
+      auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+      ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+      EXPECT_TRUE(future.get());
+      received_msg_promise = {};
+    }
+
+    // 2. use rclcpp::get_logger
+    {
+      RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str());
+      auto future = received_msg_promise.get_future();
+      auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+      ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+      EXPECT_TRUE(future.get());
+      received_msg_promise = {};
+    }
+  }
+
+  // `child_logger` is end of life, there is no sublogger
+  {
+    RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str());
+    auto future = received_msg_promise.get_future();
+    auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+    ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code);
+    received_msg_promise = {};
+  }
+}
+
+TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) {
+  std::string logger_name = "ns.test_rosout_subscription";
+  this->rosout_msg_data = "SOMETHING";
+  this->rosout_msg_name = logger_name;
+
+  rclcpp::Logger logger = this->node->get_logger();
+  ASSERT_EQ(logger.get_name(), logger_name);
+  RCLCPP_INFO(logger, this->rosout_msg_data.c_str());
+  auto future = received_msg_promise.get_future();
+  auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(future.get());
+  received_msg_promise = {};
+}
+
+TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) {
+  std::string logger_name = "ns.test_rosout_subscription.child1";
+  this->rosout_msg_data = "SOMETHING";
+  this->rosout_msg_name = logger_name;
+
+  rclcpp::Logger logger = this->node->get_logger();
+  RCLCPP_INFO(logger, this->rosout_msg_data.c_str());
+  auto future = received_msg_promise.get_future();
+  auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code);
+  received_msg_promise = {};
+
+  logger = this->node->get_logger().get_child("child1");
+  RCLCPP_INFO(logger, this->rosout_msg_data.c_str());
+  future = received_msg_promise.get_future();
+  return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(future.get());
+  received_msg_promise = {};
+
+  logger = this->node->get_logger().get_child("child2");
+  RCLCPP_INFO(logger, this->rosout_msg_data.c_str());
+  future = received_msg_promise.get_future();
+  return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code);
+  received_msg_promise = {};
+
+  this->rosout_msg_name = "ns.test_rosout_subscription.child2";
+  RCLCPP_INFO(logger, this->rosout_msg_data.c_str());
+  future = received_msg_promise.get_future();
+  return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(future.get());
+  received_msg_promise = {};
+}
+
+TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) {
+  std::string logger_name = "ns.test_rosout_subscription.child.grandchild";
+  this->rosout_msg_data = "SOMETHING";
+  this->rosout_msg_name = logger_name;
+
+  rclcpp::Logger grandchild_logger =
+    this->node->get_logger().get_child("child").get_child("grandchild");
+  ASSERT_EQ(grandchild_logger.get_name(), logger_name);
+  RCLCPP_INFO(grandchild_logger, this->rosout_msg_data.c_str());
+  auto future = received_msg_promise.get_future();
+  auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(future.get());
+  received_msg_promise = {};
+}

rclcpp/test/rclcpp/test_serialized_message.cpp

@@ -67,8 +67,6 @@ TEST(TestSerializedMessage, various_constructors) {
   rclcpp::SerializedMessage yet_another_serialized_message(std::move(other_serialized_message));
   auto & yet_another_rcl_handle = yet_another_serialized_message.get_rcl_serialized_message();
   EXPECT_TRUE(nullptr == other_rcl_handle.buffer);
-  EXPECT_EQ(0u, other_serialized_message.capacity());
-  EXPECT_EQ(0u, other_serialized_message.size());
   EXPECT_TRUE(nullptr != yet_another_rcl_handle.buffer);
   EXPECT_EQ(content_size, yet_another_serialized_message.size());
   EXPECT_EQ(content_size, yet_another_serialized_message.capacity());

rclcpp/test/rclcpp/test_service_introspection.cpp

@@ -0,0 +1,339 @@
+// 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 <rcl/service_introspection.h>
+#include <rmw/rmw.h>
+
+#include <map>
+#include <string>
+
+#include "gmock/gmock.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/node_options.hpp"
+#include "rclcpp/parameter.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+#include "test_msgs/srv/basic_types.hpp"
+#include "service_msgs/msg/service_event_info.hpp"
+
+using namespace std::chrono_literals;
+using test_msgs::srv::BasicTypes;
+using service_msgs::msg::ServiceEventInfo;
+
+
+class TestServiceIntrospection : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>(
+      "my_node", "/ns");
+
+    auto srv_callback =
+      [](const BasicTypes::Request::SharedPtr & req, const BasicTypes::Response::SharedPtr & resp) {
+        resp->set__bool_value(!req->bool_value);
+        resp->set__int64_value(req->int64_value);
+        return resp;
+      };
+
+    auto callback = [this](const std::shared_ptr<const BasicTypes::Event> & msg) {
+        events.push_back(msg);
+        (void)msg;
+      };
+
+    client = node->create_client<BasicTypes>("service");
+    service = node->create_service<BasicTypes>("service", srv_callback);
+    sub = node->create_subscription<BasicTypes::Event>("service/_service_event", 10, callback);
+    events.clear();
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Client<BasicTypes>::SharedPtr client;
+  rclcpp::Service<BasicTypes>::SharedPtr service;
+  rclcpp::Subscription<BasicTypes::Event>::SharedPtr sub;
+  std::vector<std::shared_ptr<const BasicTypes::Event>> events;
+  std::chrono::milliseconds timeout = std::chrono::milliseconds(1000);
+};
+
+TEST_F(TestServiceIntrospection, service_introspection_nominal)
+{
+  auto request = std::make_shared<BasicTypes::Request>();
+  request->set__bool_value(true);
+  request->set__int64_value(42);
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+
+  auto future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+
+  BasicTypes::Response::SharedPtr response = future.get();
+  ASSERT_EQ(response->bool_value, false);
+  ASSERT_EQ(response->int64_value, 42);
+
+  // wrap up work to get all the service_event messages
+  auto start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+
+  std::map<uint8_t, std::shared_ptr<const BasicTypes::Event>> event_map;
+  for (auto & event : events) {
+    event_map[event->info.event_type] = event;
+  }
+  ASSERT_EQ(event_map.size(), 4U);
+
+  rmw_gid_t client_gid;
+  rmw_get_gid_for_client(rcl_client_get_rmw_handle(client->get_client_handle().get()), &client_gid);
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> client_gid_arr;
+  std::move(std::begin(client_gid.data), std::end(client_gid.data), client_gid_arr.begin());
+  ASSERT_THAT(
+    client_gid_arr,
+    testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid));
+
+  ASSERT_EQ(
+    event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number,
+    event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number);
+  ASSERT_EQ(
+    event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number,
+    event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number);
+  ASSERT_EQ(
+    event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number,
+    event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number);
+  ASSERT_EQ(
+    event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number,
+    event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number);
+
+  ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].int64_value, 42);
+  ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].bool_value, true);
+  ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].int64_value, 42);
+  ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].bool_value, false);
+  ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->request.size(), 0U);
+  ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_RECEIVED]->response.size(), 0U);
+}
+
+TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events)
+{
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
+
+  auto request = std::make_shared<BasicTypes::Request>();
+  request->set__bool_value(true);
+  request->set__int64_value(42);
+  auto future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  auto start = std::chrono::steady_clock::now();
+  while ((std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 0U);
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 2U);
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 2U);
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 4U);
+}
+
+TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content)
+{
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+
+  auto request = std::make_shared<BasicTypes::Request>();
+  request->set__bool_value(true);
+  request->set__int64_value(42);
+  auto future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  auto start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 4U);
+  for (const auto & event : events) {
+    EXPECT_EQ(event->request.size(), 0U);
+    EXPECT_EQ(event->response.size(), 0U);
+  }
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 4U);
+  for (const auto & event : events) {
+    switch (event->info.event_type) {
+      case ServiceEventInfo::REQUEST_SENT:
+        EXPECT_EQ(event->request.size(), 1U);
+        break;
+      case ServiceEventInfo::REQUEST_RECEIVED:
+        EXPECT_EQ(event->request.size(), 0U);
+        break;
+      case ServiceEventInfo::RESPONSE_SENT:
+        EXPECT_EQ(event->response.size(), 0U);
+        break;
+      case ServiceEventInfo::RESPONSE_RECEIVED:
+        EXPECT_EQ(event->response.size(), 1U);
+        break;
+    }
+  }
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 4U);
+  for (const auto & event : events) {
+    switch (event->info.event_type) {
+      case ServiceEventInfo::REQUEST_SENT:
+        EXPECT_EQ(event->request.size(), 0U);
+        break;
+      case ServiceEventInfo::REQUEST_RECEIVED:
+        EXPECT_EQ(event->request.size(), 1U);
+        break;
+      case ServiceEventInfo::RESPONSE_SENT:
+        EXPECT_EQ(event->response.size(), 1U);
+        break;
+      case ServiceEventInfo::RESPONSE_RECEIVED:
+        EXPECT_EQ(event->response.size(), 0U);
+        break;
+    }
+  }
+
+  events.clear();
+
+  client->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+  service->configure_introspection(
+    node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
+
+  future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future, timeout));
+  start = std::chrono::steady_clock::now();
+  while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_EQ(events.size(), 4U);
+  for (const auto & event : events) {
+    switch (event->info.event_type) {
+      case ServiceEventInfo::REQUEST_SENT:
+      case ServiceEventInfo::REQUEST_RECEIVED:
+        EXPECT_EQ(event->request.size(), 1U);
+        break;
+      case ServiceEventInfo::RESPONSE_SENT:
+      case ServiceEventInfo::RESPONSE_RECEIVED:
+        EXPECT_EQ(event->response.size(), 1U);
+        break;
+    }
+  }
+}

rclcpp/test/rclcpp/test_time_source.cpp

@@ -81,7 +81,7 @@ void spin_until_time(
 
     executor.spin_once(10ms);
 
-    if (clock->now().nanoseconds() >= end_time.count()) {
+    if (clock->now().nanoseconds() == end_time.count()) {
       return;
     }
   }

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,18 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+19.3.0 (2023-03-01)
+-------------------
+
+19.2.0 (2023-02-24)
+-------------------
+
+19.1.0 (2023-02-14)
+-------------------
+
+19.0.0 (2023-01-30)
+-------------------
+
 18.0.0 (2022-12-29)
 -------------------
 * Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)

rclcpp_action/CMakeLists.txt

@@ -9,9 +9,10 @@ find_package(rcl_action REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(

rclcpp_action/include/rclcpp_action/types.hpp

@@ -34,7 +34,7 @@ using GoalUUID = std::array<uint8_t, UUID_SIZE>;
 using GoalStatus = action_msgs::msg::GoalStatus;
 using GoalInfo = action_msgs::msg::GoalInfo;
 
-/// Convert a goal id to a human readable string.
+/// Convert a goal id to a human readable RFC-4122 compliant string.
 RCLCPP_ACTION_PUBLIC
 std::string
 to_string(const GoalUUID & goal_id);

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>18.0.0</version>
+  <version>19.3.0</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_action/src/client.cpp

@@ -318,14 +318,19 @@ ClientBase::handle_result_response(
   const rmw_request_id_t & response_header,
   std::shared_ptr<void> response)
 {
-  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
-  const int64_t & sequence_number = response_header.sequence_number;
-  if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
-    RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
-    return;
+  std::map<int64_t, ResponseCallback>::node_type pending_result_response;
+  {
+    std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+    const int64_t & sequence_number = response_header.sequence_number;
+    if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
+      RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
+      return;
+    }
+    pending_result_response =
+      pimpl_->pending_result_responses.extract(sequence_number);
   }
-  pimpl_->pending_result_responses[sequence_number](response);
-  pimpl_->pending_result_responses.erase(sequence_number);
+  auto & response_callback = pending_result_response.mapped();
+  response_callback(response);
 }
 
 void

rclcpp_action/src/types.cpp

@@ -15,25 +15,33 @@
 #include "rclcpp_action/types.hpp"
 
 #include <string>
-#include <sstream>
 
 namespace rclcpp_action
 {
 std::string
 to_string(const GoalUUID & goal_id)
 {
-  std::stringstream stream;
-  stream << std::hex;
-  for (const auto & element : goal_id) {
-    stream << static_cast<int>(element);
+  constexpr char HEX[] = "0123456789abcdef";
+  std::string result;
+  result.resize(36);
+  size_t i = 0;
+  for (uint8_t byte : goal_id) {
+    result[i++] = HEX[byte >> 4];
+    result[i++] = HEX[byte & 0x0f];
+    // A RFC-4122 compliant UUID looks like:
+    // 00000000-0000-0000-0000-000000000000
+    // That means that there is a '-' at offset 8, 13, 18, and 23
+    if (i == 8 || i == 13 || i == 18 || i == 23) {
+      result[i++] = '-';
+    }
   }
-  return stream.str();
+  return result;
 }
 
 void
 convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
 {
-  for (size_t i = 0; i < 16; ++i) {
+  for (size_t i = 0; i < UUID_SIZE; ++i) {
     info->goal_id.uuid[i] = goal_id[i];
   }
 }
@@ -41,7 +49,7 @@ convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
 void
 convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id)
 {
-  for (size_t i = 0; i < 16; ++i) {
+  for (size_t i = 0; i < UUID_SIZE; ++i) {
     (*goal_id)[i] = info.goal_id.uuid[i];
   }
 }

rclcpp_action/test/test_types.cpp

@@ -22,17 +22,17 @@ TEST(TestActionTypes, goal_uuid_to_string) {
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
     goal_id[i] = i;
   }
-  EXPECT_STREQ("0123456789abcdef", rclcpp_action::to_string(goal_id).c_str());
+  EXPECT_STREQ("00010203-0405-0607-0809-0a0b0c0d0e0f", rclcpp_action::to_string(goal_id).c_str());
 
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
     goal_id[i] = static_cast<uint8_t>(16u + i);
   }
-  EXPECT_STREQ("101112131415161718191a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str());
+  EXPECT_STREQ("10111213-1415-1617-1819-1a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str());
 
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
     goal_id[i] = static_cast<uint8_t>(std::numeric_limits<uint8_t>::max() - i);
   }
-  EXPECT_STREQ("fffefdfcfbfaf9f8f7f6f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str());
+  EXPECT_STREQ("fffefdfc-fbfa-f9f8-f7f6-f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str());
 }
 
 TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) {
@@ -54,7 +54,7 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) {
   }
 
   rclcpp_action::GoalUUID goal_id;
-  rclcpp_action::convert(goal_id, &goal_info);
+  rclcpp_action::convert(goal_info, &goal_id);
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
     EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]);
   }

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,20 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+19.3.0 (2023-03-01)
+-------------------
+
+19.2.0 (2023-02-24)
+-------------------
+
+19.1.0 (2023-02-14)
+-------------------
+
+19.0.0 (2023-01-30)
+-------------------
+* Improve component_manager_isolated shutdown (`#2085 <https://github.com/ros2/rclcpp/issues/2085>`_)
+* Contributors: Michael Carroll
+
 18.0.0 (2022-12-29)
 -------------------
 * Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)

rclcpp_components/CMakeLists.txt

@@ -2,9 +2,10 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp_components)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(

rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp

@@ -38,6 +38,16 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager
   {
     std::shared_ptr<rclcpp::Executor> executor;
     std::thread thread;
+    std::atomic_bool thread_initialized;
+
+    /// Constructor for the wrapper.
+    /// This is necessary as atomic variables don't have copy/move operators
+    /// implemented so this structure is not copyable/movable by default
+    explicit DedicatedExecutorWrapper(std::shared_ptr<rclcpp::Executor> exec)
+    : executor(exec),
+      thread_initialized(false)
+    {
+    }
   };
 
 public:
@@ -59,15 +69,19 @@ protected:
   void
   add_node_to_executor(uint64_t node_id) override
   {
-    DedicatedExecutorWrapper executor_wrapper;
     auto exec = std::make_shared<ExecutorT>();
     exec->add_node(node_wrappers_[node_id].get_node_base_interface());
-    executor_wrapper.executor = exec;
-    executor_wrapper.thread = std::thread(
-      [exec]() {
+
+    // Emplace rather than std::move since move operations are deleted for atomics
+    auto result = dedicated_executor_wrappers_.emplace(std::make_pair(node_id, exec));
+    DedicatedExecutorWrapper & wrapper = result.first->second;
+    wrapper.executor = exec;
+    auto & thread_initialized = wrapper.thread_initialized;
+    wrapper.thread = std::thread(
+      [exec, &thread_initialized]() {
+        thread_initialized = true;
         exec->spin();
       });
-    dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper);
   }
   /// Remove component node from executor model, it's invoked in on_unload_node()
   /**
@@ -90,15 +104,21 @@ private:
    */
   void cancel_executor(DedicatedExecutorWrapper & executor_wrapper)
   {
-    // We can't immediately call the cancel() API on an executor because if it is not
-    // already spinning, this operation will have no effect.
-    // We rely on the assumption that this class creates executors and then immediately makes them
-    // spin in a separate thread, i.e. the time gap between when the executor is created and when
-    // it starts to spin is small (although it's not negligible).
+    // Verify that the executor thread has begun spinning.
+    // If it has not, then wait until the thread starts to ensure
+    // that cancel() will fully stop the execution
+    //
+    // This prevents a previous race condition that occurs between the
+    // creation of the executor spin thread and cancelling an executor
 
-    while (!executor_wrapper.executor->is_spinning()) {
-      // This is an arbitrarily small delay to avoid busy looping
-      rclcpp::sleep_for(std::chrono::milliseconds(1));
+    if (!executor_wrapper.thread_initialized) {
+      auto context = this->get_node_base_interface()->get_context();
+
+      // Guarantee that either the executor is spinning or we are shutting down.
+      while (!executor_wrapper.executor->is_spinning() && rclcpp::ok(context)) {
+        // This is an arbitrarily small delay to avoid busy looping
+        rclcpp::sleep_for(std::chrono::milliseconds(1));
+      }
     }
 
     // After the while loop we are sure that the executor is now spinning, so

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>18.0.0</version>
+  <version>19.3.0</version>
   <description>Package containing tools for dynamically loadable components</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,18 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+19.3.0 (2023-03-01)
+-------------------
+
+19.2.0 (2023-02-24)
+-------------------
+
+19.1.0 (2023-02-14)
+-------------------
+
+19.0.0 (2023-01-30)
+-------------------
+
 18.0.0 (2022-12-29)
 -------------------
 * Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)

rclcpp_lifecycle/CMakeLists.txt

@@ -2,9 +2,10 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp_lifecycle)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -172,6 +172,15 @@ public:
   const char *
   get_namespace() const;
 
+  /// Get the fully-qualified name of the node.
+  /**
+   * The fully-qualified name includes the local namespace and name of the node.
+   * \return fully-qualified name of the node.
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  const char *
+  get_fully_qualified_name() const;
+
   /// Get the logger of the node.
   /**
    * \return The logger of the node.

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>18.0.0</version>
+  <version>19.3.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -165,6 +165,12 @@ LifecycleNode::get_namespace() const
   return node_base_->get_namespace();
 }
 
+const char *
+LifecycleNode::get_fully_qualified_name() const
+{
+  return node_base_->get_fully_qualified_name();
+}
+
 rclcpp::Logger
 LifecycleNode::get_logger() const
 {

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -229,6 +229,7 @@ TEST_F(TestDefaultStateMachine, empty_initializer) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
   EXPECT_STREQ("testnode", test_node->get_name());
   EXPECT_STREQ("/", test_node->get_namespace());
+  EXPECT_STREQ("/testnode", test_node->get_fully_qualified_name());
   EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
 }
 

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -133,6 +133,7 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) {
     rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
+  (void)ret;  // Just to make clang happy
   EXPECT_FALSE(node_->publisher()->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();