Implement dynamic subscription and C++ wrapper classes

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

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

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

CODEOWNERS

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,139 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+29.3.0 (2024-12-20)
+-------------------
+* Fix transient local IPC publish  (`#2708 <https://github.com/ros2/rclcpp/issues/2708>`_)
+* apply actual QoS from rmw to the IPC publisher. (`#2707 <https://github.com/ros2/rclcpp/issues/2707>`_)
+* Adding in topic name to logging on IPC issues (`#2706 <https://github.com/ros2/rclcpp/issues/2706>`_)
+* fix TestTimeSource.ROS_time_valid_attach_detach. (`#2700 <https://github.com/ros2/rclcpp/issues/2700>`_)
+* Update docstring for `rclcpp::Node::now()` (`#2696 <https://github.com/ros2/rclcpp/issues/2696>`_)
+* Re-enable executor test on rmw_connextdds. (`#2693 <https://github.com/ros2/rclcpp/issues/2693>`_)
+* Fix warnings on Windows. (`#2692 <https://github.com/ros2/rclcpp/issues/2692>`_)
+* Omnibus fixes for running tests with Connext. (`#2684 <https://github.com/ros2/rclcpp/issues/2684>`_)
+* fix(Executor): Fix segfault if callback group is deleted during rmw_wait (`#2683 <https://github.com/ros2/rclcpp/issues/2683>`_)
+* Contributors: Chris Lalancette, Jeffery Hsu, Patrick Roncagliolo, Steve Macenski, Tomoya Fujita, jmachowinski
+
+29.2.0 (2024-11-25)
+-------------------
+* accept custom allocator for LoanedMessage. (`#2672 <https://github.com/ros2/rclcpp/issues/2672>`_)
+* Contributors: Tomoya Fujita
+
+29.1.0 (2024-11-20)
+-------------------
+* a couple of typo fixes in doc section for LoanedMessage. (`#2676 <https://github.com/ros2/rclcpp/issues/2676>`_)
+* Make sure callback_end tracepoint is triggered in AnyServiceCallback (`#2670 <https://github.com/ros2/rclcpp/issues/2670>`_)
+* Correct the incorrect comments in generic_client.hpp (`#2662 <https://github.com/ros2/rclcpp/issues/2662>`_)
+* Fix NodeOptions assignment operator (`#2656 <https://github.com/ros2/rclcpp/issues/2656>`_)
+* set QoS History KEEP_ALL explicitly for statistics publisher. (`#2650 <https://github.com/ros2/rclcpp/issues/2650>`_)
+* Fix test_intra_process_manager.cpp with rmw_zenoh_cpp (`#2653 <https://github.com/ros2/rclcpp/issues/2653>`_)
+* Fixed test_events_executors in zenoh (`#2643 <https://github.com/ros2/rclcpp/issues/2643>`_)
+* rmw_fastrtps supports service event gid uniqueness test. (`#2638 <https://github.com/ros2/rclcpp/issues/2638>`_)
+* print warning if event callback is not supported instead of passing exception. (`#2648 <https://github.com/ros2/rclcpp/issues/2648>`_)
+* Implement callback support of async_send_request for service generic client (`#2614 <https://github.com/ros2/rclcpp/issues/2614>`_)
+* Contributors: Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Romain DESILLE, Tomoya Fujita
+
+29.0.0 (2024-10-03)
+-------------------
+* Fixed test qos rmw zenoh (`#2639 <https://github.com/ros2/rclcpp/issues/2639>`_)
+* verify client gid uniqueness for a single service event. (`#2636 <https://github.com/ros2/rclcpp/issues/2636>`_)
+* Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (`#2626 <https://github.com/ros2/rclcpp/issues/2626>`_)
+* Shutdown the context before context's destructor is invoked in tests (`#2633 <https://github.com/ros2/rclcpp/issues/2633>`_)
+* Skip rmw zenoh content filtering tests (`#2627 <https://github.com/ros2/rclcpp/issues/2627>`_)
+* Use InvalidServiceTypeError for unavailable service type in GenericClient (`#2629 <https://github.com/ros2/rclcpp/issues/2629>`_)
+* Implement generic service (`#2617 <https://github.com/ros2/rclcpp/issues/2617>`_)
+* fix events-executor warm-up bug and add unit-tests (`#2591 <https://github.com/ros2/rclcpp/issues/2591>`_)
+* remove unnecessary gtest-skip in test_executors (`#2600 <https://github.com/ros2/rclcpp/issues/2600>`_)
+* Correct node name in service test code (`#2615 <https://github.com/ros2/rclcpp/issues/2615>`_)
+* Minor naming fixes for ParameterValue to_string() function (`#2609 <https://github.com/ros2/rclcpp/issues/2609>`_)
+* Removed clang warnings (`#2605 <https://github.com/ros2/rclcpp/issues/2605>`_)
+* Fix a couple of issues in the documentation. (`#2608 <https://github.com/ros2/rclcpp/issues/2608>`_)
+* deprecate the static single threaded executor (`#2598 <https://github.com/ros2/rclcpp/issues/2598>`_)
+* Fix name of ParameterEventHandler class in doc (`#2604 <https://github.com/ros2/rclcpp/issues/2604>`_)
+* subscriber_statistics_collectors\_ should be protected by mutex. (`#2592 <https://github.com/ros2/rclcpp/issues/2592>`_)
+* Fix bug in timers lifecycle for events executor (`#2586 <https://github.com/ros2/rclcpp/issues/2586>`_)
+* fix rclcpp/test/rclcpp/CMakeLists.txt to check for the correct targets existance (`#2596 <https://github.com/ros2/rclcpp/issues/2596>`_)
+* Shut down context during init if logging config fails (`#2594 <https://github.com/ros2/rclcpp/issues/2594>`_)
+* Make more of the Waitable API abstract (`#2593 <https://github.com/ros2/rclcpp/issues/2593>`_)
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Alexis Pojomovsky, Barry Xu, Chris Lalancette, Christophe Bedard, Kang, Hsin-Yi, Tomoya Fujita
+
+28.3.3 (2024-07-29)
+-------------------
+* Only compile the tests once. (`#2590 <https://github.com/ros2/rclcpp/issues/2590>`_)
+* Contributors: Chris Lalancette
+
+28.3.2 (2024-07-24)
+-------------------
+* Updated rcpputils path API (`#2579 <https://github.com/ros2/rclcpp/issues/2579>`_)
+* Make the subscriber_triggered_to_receive_message test more reliable. (`#2584 <https://github.com/ros2/rclcpp/issues/2584>`_)
+  * Make the subscriber_triggered_to_receive_message test more reliable.
+  In the current code, inside of the timer we create the subscription
+  and the publisher, publish immediately, and expect the subscription
+  to get it immediately.  But it may be the case that discovery
+  hasn't even happened between the publisher and the subscription
+  by the time the publish call happens.
+  To make this more reliable, create the subscription and publish *before*
+  we ever create and spin on the timer.  This at least gives 100
+  milliseconds for discovery to happen.  That may not be quite enough
+  to make this reliable on all platforms, but in my local testing this
+  helps a lot.  Prior to this change I can make this fail one out of 10
+  times, and after the change I've run 100 times with no failures.
+* Have the EventsExecutor use more common code  (`#2570 <https://github.com/ros2/rclcpp/issues/2570>`_)
+  * move notify waitable setup to its own function
+  * move mutex lock to retrieve_entity utility
+  * use entities_need_rebuild\_ atomic bool in events-executors
+  * remove duplicated set_on_ready_callback for notify_waitable
+  * use mutex from base class rather than a new recursive mutex
+  * use current_collection\_ member in events-executor
+  * delay adding notify waitable to collection
+  * postpone clearing the current collection
+  * commonize notify waitable and collection
+  * commonize add/remove node/cbg methods
+  * fix linter errors
+  ---------
+* Removed deprecated methods and classes (`#2575 <https://github.com/ros2/rclcpp/issues/2575>`_)
+* Release ownership of entities after spinning cancelled (`#2556 <https://github.com/ros2/rclcpp/issues/2556>`_)
+  * Release ownership of entities after spinning cancelled
+  * Move release action to every exit point in different spin functions
+  * Move wait_result\_.reset() before setting spinning to false
+  * Update test code
+  * Move test code to test_executors.cpp
+  ---------
+* Split test_executors.cpp even further. (`#2572 <https://github.com/ros2/rclcpp/issues/2572>`_)
+  That's because it is too large for Windows Debug to compile,
+  so split into smaller bits.
+  Even with this split, the file is too big; that's likely
+  because we are using TYPED_TEST here, which generates multiple
+  symbols per test case.  To deal with this, without further
+  breaking up the file, also add in the /bigobj flag when
+  compiling on Windows Debug.
+* avoid adding notify waitable twice to events-executor collection (`#2564 <https://github.com/ros2/rclcpp/issues/2564>`_)
+  * avoid adding notify waitable twice to events-executor entities collection
+  * remove redundant mutex lock
+  ---------
+* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette
+
+28.3.1 (2024-06-25)
+-------------------
+* Remove unnecessary msg includes in tests (`#2566 <https://github.com/ros2/rclcpp/issues/2566>`_)
+* Fix copy-paste errors in function docs (`#2565 <https://github.com/ros2/rclcpp/issues/2565>`_)
+* Fix typo in function doc (`#2563 <https://github.com/ros2/rclcpp/issues/2563>`_)
+* Contributors: Christophe Bedard
+
+28.3.0 (2024-06-17)
+-------------------
+* Add test creating two content filter topics with the same topic name (`#2546 <https://github.com/ros2/rclcpp/issues/2546>`_) (`#2549 <https://github.com/ros2/rclcpp/issues/2549>`_)
+* add impl pointer for ExecutorOptions (`#2523 <https://github.com/ros2/rclcpp/issues/2523>`_)
+* Fixup Executor::spin_all() regression fix (`#2517 <https://github.com/ros2/rclcpp/issues/2517>`_)
+* Add 'mimick' label to tests which use Mimick (`#2516 <https://github.com/ros2/rclcpp/issues/2516>`_)
+* Contributors: Alejandro Hernández Cordero, Scott K Logan, William Woodall
+
+28.2.0 (2024-04-26)
+-------------------
+* Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 <https://github.com/ros2/rclcpp/issues/2510>`_)
+* Revise the description of service configure_introspection() (`#2511 <https://github.com/ros2/rclcpp/issues/2511>`_)
+* Contributors: Barry Xu, Sharmin Ramli
+
 28.1.0 (2024-04-16)
 -------------------
 * Remove references to index.ros.org. (`#2504 <https://github.com/ros2/rclcpp/issues/2504>`_)

rclcpp/CMakeLists.txt

@@ -63,6 +63,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/exceptions/exceptions.cpp
   src/rclcpp/executable_list.cpp
   src/rclcpp/executor.cpp
+  src/rclcpp/executor_options.cpp
   src/rclcpp/executors.cpp
   src/rclcpp/executors/executor_entities_collection.cpp
   src/rclcpp/executors/executor_entities_collector.cpp
@@ -76,6 +77,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/future_return_code.cpp
   src/rclcpp/generic_client.cpp
   src/rclcpp/generic_publisher.cpp
+  src/rclcpp/generic_service.cpp
   src/rclcpp/generic_subscription.cpp
   src/rclcpp/graph_listener.cpp
   src/rclcpp/guard_condition.cpp
@@ -111,6 +113,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/qos.cpp
   src/rclcpp/event_handler.cpp
   src/rclcpp/qos_overriding_options.cpp
+  src/rclcpp/dynamic_subscription.cpp
   src/rclcpp/rate.cpp
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -165,11 +165,13 @@ public:
     if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
       const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
       cb(request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
       return nullptr;
     }
     if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
       const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
       cb(service_handle, request_header, std::move(request));
+      TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
       return nullptr;
     }
     // auto response = allocate_shared<typename ServiceT::Response, Allocator>();

rclcpp/include/rclcpp/callback_group.hpp

@@ -59,46 +59,6 @@ class CallbackGroup
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
-  /// Constructor for CallbackGroup.
-  /**
-   * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
-   * and when creating one the type must be specified.
-   *
-   * Callbacks in Reentrant Callback Groups must be able to:
-   *   - run at the same time as themselves (reentrant)
-   *   - run at the same time as other callbacks in their group
-   *   - run at the same time as other callbacks in other groups
-   *
-   * Callbacks in Mutually Exclusive Callback Groups:
-   *   - will not be run multiple times simultaneously (non-reentrant)
-   *   - will not be run at the same time as other callbacks in their group
-   *   - but must run at the same time as callbacks in other groups
-   *
-   * Additionally, callback groups have a property which determines whether or
-   * not they are added to an executor with their associated node automatically.
-   * When creating a callback group the automatically_add_to_executor_with_node
-   * argument determines this behavior, and if true it will cause the newly
-   * created callback group to be added to an executor with the node when the
-   * Executor::add_node method is used.
-   * If false, this callback group will not be added automatically and would
-   * have to be added to an executor manually using the
-   * Executor::add_callback_group method.
-   *
-   * Whether the node was added to the executor before creating the callback
-   * group, or after, is irrelevant; the callback group will be automatically
-   * added to the executor in either case.
-   *
-   * \param[in] group_type The type of the callback group.
-   * \param[in] automatically_add_to_executor_with_node A boolean that
-   *   determines whether a callback group is automatically added to an executor
-   *   with the node with which it is associated.
-   */
-  [[deprecated("Use CallbackGroup constructor with context function argument")]]
-  RCLCPP_PUBLIC
-  explicit CallbackGroup(
-    CallbackGroupType group_type,
-    bool automatically_add_to_executor_with_node = true);
-
   /// Constructor for CallbackGroup.
   /**
    * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'

rclcpp/include/rclcpp/client.hpp

@@ -70,14 +70,6 @@ struct FutureAndRequestId
   /// Allow implicit conversions to `std::future` by reference.
   operator FutureT &() {return this->future;}
 
-  /// Deprecated, use the `future` member variable instead.
-  /**
-   * Allow implicit conversions to `std::future` by value.
-   * \deprecated
-   */
-  [[deprecated("FutureAndRequestId: use .future instead of an implicit conversion")]]
-  operator FutureT() {return this->future;}
-
   // delegate future like methods in the std::future impl_
 
   /// See std::future::get().
@@ -436,15 +428,6 @@ public:
   {
     using detail::FutureAndRequestId<std::future<SharedResponse>>::FutureAndRequestId;
 
-    /// Deprecated, use `.future.share()` instead.
-    /**
-     * Allow implicit conversions to `std::shared_future` by value.
-     * \deprecated
-     */
-    [[deprecated(
-      "FutureAndRequestId: use .future.share() instead of an implicit conversion")]]
-    operator SharedFuture() {return this->future.share();}
-
     // delegate future like methods in the std::future impl_
 
     /// See std::future::share().
@@ -490,7 +473,7 @@ public:
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] node_graph The node graph interface of the corresponding node.
    * \param[in] service_name Name of the topic to publish to.
-   * \param[in] client_options options for the subscription.
+   * \param[in] client_options options for the client.
    */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,

rclcpp/include/rclcpp/clock.hpp

@@ -217,13 +217,13 @@ public:
   std::mutex &
   get_clock_mutex() noexcept;
 
-  // Add a callback to invoke if the jump threshold is exceeded.
+  /// Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the
    * returned shared pointer is valid.
    *
    * Function will register callbacks to the callback queue. On time jump all
-   * callbacks will be executed whose threshold is greater then the time jump;
+   * callbacks will be executed whose threshold is greater than the time jump;
    * The logic will first call selected pre_callbacks and then all selected
    * post_callbacks.
    *
@@ -232,7 +232,7 @@ public:
    * \param pre_callback Must be non-throwing
    * \param post_callback Must be non-throwing.
    * \param threshold Callbacks will be triggered if the time jump is greater
-   * then the threshold.
+   * than the threshold.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    * \throws std::bad_alloc if the allocation of the JumpHandler fails.
    * \warning the instance of the clock must remain valid as long as any created

rclcpp/include/rclcpp/create_client.hpp

@@ -47,28 +47,9 @@ create_client(
   const std::string & service_name,
   const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
   rclcpp::CallbackGroup::SharedPtr group = nullptr)
-{
-  return create_client<ServiceT>(
-    node_base, node_graph, node_services,
-    service_name,
-    qos.get_rmw_qos_profile(),
-    group);
-}
-
-/// Create a service client with a given type.
-/// \internal
-template<typename ServiceT>
-typename rclcpp::Client<ServiceT>::SharedPtr
-create_client(
-  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
-  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-  const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
-  options.qos = qos_profile;
+  options.qos = qos.get_rmw_qos_profile();
 
   auto cli = rclcpp::Client<ServiceT>::make_shared(
     node_base.get(),
@@ -80,7 +61,6 @@ create_client(
   node_services->add_client(cli_base_ptr, group);
   return cli;
 }
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_CLIENT_HPP_

rclcpp/include/rclcpp/create_generic_service.hpp

@@ -0,0 +1,102 @@
+// Copyright 2024 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.
+
+#ifndef RCLCPP__CREATE_GENERIC_SERVICE_HPP_
+#define RCLCPP__CREATE_GENERIC_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/generic_service.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+/// Create a generic service with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the generic service.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+create_generic_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rclcpp::GenericServiceCallback any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos.get_rmw_qos_profile();
+
+  auto serv = GenericService::make_shared(
+    node_base->get_shared_rcl_node_handle(),
+    service_name, service_type, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+/// Create a generic service with a given type.
+/**
+ * The NodeT type needs to have NodeBaseInterface implementation and NodeServicesInterface
+ * implementation of the node which to create the generic service.
+ *
+ * \param[in] node The node on which to create the generic service.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+ * \param[in] callback The callback to call when the service gets a request.
+ * \param[in] qos Quality of service profile for the service.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created service.
+ */
+template<typename NodeT, typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+create_generic_service(
+  NodeT node,
+  const std::string & service_name,
+  const std::string & service_type,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return create_generic_service<CallbackT>(
+    rclcpp::node_interfaces::get_node_base_interface(node),
+    rclcpp::node_interfaces::get_node_services_interface(node),
+    service_name,
+    service_type,
+    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+}
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_SERVICE_HPP_

rclcpp/include/rclcpp/dynamic_subscription.hpp

@@ -0,0 +1,175 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
+#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
+
+#include <rosidl_dynamic_typesupport/identifier.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcpputils/shared_library.hpp"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/typesupport_helpers.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// %Subscription for messages whose type descriptions are obtained at runtime.
+/**
+ * Since the type is not known at compile time, this is not a template, and the dynamic library
+ * containing type support information has to be identified and loaded based on the type name.
+ *
+ * NOTE(methylDragon): No considerations for intra-process handling are made.
+ */
+class DynamicSubscription : public rclcpp::SubscriptionBase
+{
+public:
+  // cppcheck-suppress unknownMacro
+  RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription)
+
+  template<typename AllocatorT = std::allocator<void>>
+  DynamicSubscription(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support,
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    std::function<void(
+      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
+      const rosidl_runtime_c__type_description__TypeDescription &
+    )> callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
+  : SubscriptionBase(
+      node_base,
+      type_support->get_const_rosidl_message_type_support(),
+      topic_name,
+      options.to_rcl_subscription_options(
+        qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
+      DeliveredMessageKind::DYNAMIC_MESSAGE),
+    ts_(type_support),
+    callback_(callback),
+    serialization_support_(nullptr),
+    dynamic_message_(nullptr),
+    dynamic_message_type_(nullptr)
+  {
+    if (!type_support) {
+      throw std::runtime_error("DynamicMessageTypeSupport cannot be nullptr!");
+    }
+
+    if (type_support->get_const_rosidl_message_type_support().typesupport_identifier !=
+      rosidl_get_dynamic_typesupport_identifier())
+    {
+      throw std::runtime_error(
+              "DynamicSubscription must use dynamic type introspection type support!");
+    }
+
+    serialization_support_ = type_support->get_shared_dynamic_serialization_support();
+    dynamic_message_type_ = type_support->get_shared_dynamic_message_type()->clone_shared();
+    dynamic_message_ = type_support->get_shared_dynamic_message()->clone_shared();
+  }
+
+  // TODO(methylDragon):
+  /// Deferred type description constructor, only usable if the middleware implementation supports
+  /// type discovery
+
+  RCLCPP_PUBLIC
+  virtual ~DynamicSubscription() = default;
+
+  // Same as create_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  std::shared_ptr<void> create_message() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
+
+  /// Cast the message to a rclcpp::SerializedMessage and call the callback.
+  RCLCPP_PUBLIC
+  void handle_message(
+    std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
+
+  /// Handle dispatching rclcpp::SerializedMessage to user callback.
+  RCLCPP_PUBLIC
+  void
+  handle_serialized_message(
+    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
+    const rclcpp::MessageInfo & message_info) override;
+
+  /// This function is currently not implemented.
+  RCLCPP_PUBLIC
+  void handle_loaned_message(
+    void * loaned_message, const rclcpp::MessageInfo & message_info) override;
+
+  // Same as return_serialized_message() as the subscription is to serialized_messages only
+  RCLCPP_PUBLIC
+  void return_message(std::shared_ptr<void> & message) override;
+
+  RCLCPP_PUBLIC
+  void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
+
+
+  // DYNAMIC TYPE ==================================================================================
+  // TODO(methylDragon): Reorder later
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
+
+  RCLCPP_PUBLIC
+  void return_dynamic_message(
+    rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
+
+  RCLCPP_PUBLIC
+  void handle_dynamic_message(
+    const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+    const rclcpp::MessageInfo & message_info) override;
+
+private:
+  RCLCPP_DISABLE_COPY(DynamicSubscription)
+
+  rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_;
+  std::function<void(
+      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
+      const rosidl_runtime_c__type_description__TypeDescription &
+    )> callback_;
+
+  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_;
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_;
+  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr dynamic_message_type_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_

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

@@ -0,0 +1,327 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
+#endif
+
+
+#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicMessage::get_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id) \
+  { \
+    ValueT out; \
+    rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \
+      &rosidl_dynamic_data_, id, &out); \
+    return out; \
+  }
+
+#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  ValueT \
+  DynamicMessage::get_value<ValueT>(const std::string & name) \
+  { \
+    return get_value<ValueT>(get_member_id(name)); \
+  }
+
+#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessage::set_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \
+      &rosidl_dynamic_data_, id, value); \
+  }
+
+#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessage::set_value<ValueT>(const std::string & name, ValueT value) \
+  { \
+    set_value<ValueT>(get_member_id(name), value); \
+  }
+
+#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \
+  template<> \
+  rosidl_dynamic_typesupport_member_id_t \
+  DynamicMessage::insert_value<ValueT>(ValueT value) \
+  { \
+    rosidl_dynamic_typesupport_member_id_t out; \
+    rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \
+      &rosidl_dynamic_data_, value, &out); \
+    return out; \
+  }
+
+#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
+  __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT)
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/**
+ * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+ * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+ *
+ * Explicitly:
+ * - Basic types:        bool, byte, char
+ * - Float types:        float, double
+ * - Int types:          int8_t, int16_t, int32_t, int64_t
+ * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+ * - String types:       std::string, std::u16string
+ */
+
+DYNAMIC_MESSAGE_DEFINITIONS(bool, bool);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte);
+DYNAMIC_MESSAGE_DEFINITIONS(char, char);
+DYNAMIC_MESSAGE_DEFINITIONS(float, float32);
+DYNAMIC_MESSAGE_DEFINITIONS(double, float64);
+DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8);
+DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16);
+DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32);
+DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64);
+DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string);
+// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string);
+
+// Byte and String getters have a different implementation and are defined below
+
+
+// BYTE ============================================================================================
+template<>
+std::byte
+DynamicMessage::get_value<std::byte>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  unsigned char out;
+  rosidl_dynamic_typesupport_dynamic_data_get_byte_value(&get_rosidl_dynamic_data(), id, &out);
+  return static_cast<std::byte>(out);
+}
+
+
+template<>
+std::byte
+DynamicMessage::get_value<std::byte>(const std::string & name)
+{
+  return get_value<std::byte>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::byte>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::byte value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_byte_value(
+    &rosidl_dynamic_data_, id, static_cast<unsigned char>(value));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::byte>(const std::string & name, const std::byte value)
+{
+  set_value<std::byte>(get_member_id(name), value);
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::byte>(const std::byte value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_byte_value(
+    &rosidl_dynamic_data_, static_cast<unsigned char>(value), &out);
+  return out;
+}
+
+
+// STRINGS =========================================================================================
+template<>
+std::string
+DynamicMessage::get_value<std::string>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_string_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+template<>
+std::u16string
+DynamicMessage::get_value<std::u16string>(rosidl_dynamic_typesupport_member_id_t id)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_wstring_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+template<>
+std::string
+DynamicMessage::get_value<std::string>(const std::string & name)
+{
+  return get_value<std::string>(get_member_id(name));
+}
+
+
+template<>
+std::u16string
+DynamicMessage::get_value<std::u16string>(const std::string & name)
+{
+  return get_value<std::u16string>(get_member_id(name));
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::string>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_string_value(
+    &rosidl_dynamic_data_, id, value.c_str(), value.size());
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::u16string>(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_wstring_value(
+    &rosidl_dynamic_data_, id, value.c_str(), value.size());
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::string>(const std::string & name, const std::string value)
+{
+  set_value<std::string>(get_member_id(name), value);
+}
+
+
+template<>
+void
+DynamicMessage::set_value<std::u16string>(const std::string & name, const std::u16string value)
+{
+  set_value<std::u16string>(get_member_id(name), value);
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::string>(const std::string value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_string_value(
+    &rosidl_dynamic_data_, value.c_str(), value.size(), &out);
+  return out;
+}
+
+
+template<>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value<std::u16string>(const std::u16string value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value(
+    &rosidl_dynamic_data_, value.c_str(), value.size(), &out);
+  return out;
+}
+
+
+// THROW FOR UNSUPPORTED TYPES =====================================================================
+template<typename ValueT>
+ValueT
+DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+ValueT
+DynamicMessage::get_value(const std::string & name)
+{
+  throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+void
+DynamicMessage::set_value(
+  rosidl_dynamic_typesupport_member_id_t id, ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+void
+DynamicMessage::set_value(const std::string & name, ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
+}
+
+
+template<typename ValueT>
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_value(ValueT value)
+{
+  throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN
+#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN
+#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN
+#undef __DYNAMIC_MESSAGE_INSERT_VALUE
+#undef DYNAMIC_MESSAGE_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_

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

@@ -0,0 +1,189 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+
+#include <rosidl_dynamic_typesupport/types.h>
+#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
+
+#include <cstdint>
+#include <cstddef>
+#include <memory>
+#include <string>
+
+#include "rclcpp/exceptions.hpp"
+
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
+#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
+#endif
+
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \
+      &rosidl_dynamic_type_builder_, \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_array_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \
+    size_t array_length, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \
+      &rosidl_dynamic_type_builder_, \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
+      array_length); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_unbounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \
+    _unbounded_sequence_member( \
+      &rosidl_dynamic_type_builder_, \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
+  }
+
+#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  template<> \
+  void \
+  DynamicMessageTypeBuilder::add_bounded_sequence_member<MemberT>( \
+    rosidl_dynamic_typesupport_member_id_t id, \
+    const std::string & name, \
+    size_t sequence_bound, \
+    const std::string & default_value) \
+  { \
+    rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \
+      &rosidl_dynamic_type_builder_, \
+      id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
+      sequence_bound); \
+  }
+
+#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+  __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
+
+
+namespace rclcpp
+{
+namespace dynamic_typesupport
+{
+
+/**
+ * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+ * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+ *
+ * Explicitly:
+ * - Basic types:        bool, byte, char
+ * - Float types:        float, double
+ * - Int types:          int8_t, int16_t, int32_t, int64_t
+ * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+ * - String types:       std::string, std::u16string
+ */
+
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string);
+DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring);
+
+
+// THROW FOR UNSUPPORTED TYPES =====================================================================
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_array_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  size_t array_length, const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_array_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_unbounded_sequence_member is not implemented for input type");
+}
+
+
+template<typename MemberT>
+void
+DynamicMessageTypeBuilder::add_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id,
+  const std::string & name,
+  size_t sequence_bound,
+  const std::string & default_value)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "add_bounded_sequence_member is not implemented for input type");
+}
+
+
+}  // namespace dynamic_typesupport
+}  // namespace rclcpp
+
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN
+#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN
+#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS
+
+#endif  // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp

@@ -32,16 +32,372 @@ namespace rclcpp
 namespace dynamic_typesupport
 {
 
+class DynamicMessageType;
+class DynamicMessageTypeBuilder;
+
 /// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t
-/// STUBBED OUT
+/**
+ * This class:
+ * - Exposes getter methods for the struct
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed
+ *   DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport.
+ * - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer
+ *   must point to the same location in memory as the stored raw pointer!
+ *
+ * Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t,
+ *       even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to
+ *       rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to
+ *       rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields.
+ */
 class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
 
+  // CONSTRUCTION ==================================================================================
+  // Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the
+  // lifetime of the serialization support (if the constructor cannot otherwise get it from args).
+  //
+  // In cases where a dynamic data pointer is passed, the serialization support composed by
+  // the data should be the exact same object managed by the DynamicSerializationSupport,
+  // otherwise the lifetime management will not work properly.
+
+  /// Construct a new DynamicMessage with the provided dynamic type builder, using its allocator
+  RCLCPP_PUBLIC
+  explicit DynamicMessage(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
+
+  /// Construct a new DynamicMessage with the provided dynamic type builder and allocator
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder,
+    rcl_allocator_t allocator);
+
+  /// Construct a new DynamicMessage with the provided dynamic type, using its allocator
+  RCLCPP_PUBLIC
+  explicit DynamicMessage(std::shared_ptr<DynamicMessageType> dynamic_type);
+
+  /// Construct a new DynamicMessage with the provided dynamic type and allocator
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    std::shared_ptr<DynamicMessageType> dynamic_type,
+    rcl_allocator_t allocator);
+
+  /// Assume ownership of struct
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data);
+
+  /// Loaning constructor
+  /// Must only be called with a rosidl dynaimc data object obtained from loaning!
+  // NOTE(methylDragon): I'd put this in protected, but I need this exposed to
+  //                     enable_shared_from_this...
+  RCLCPP_PUBLIC
+  DynamicMessage(
+    DynamicMessage::SharedPtr parent_data,
+    rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data);
+
+  // NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using
+  //                     construction from dynamic type/builder, which is more efficient
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicMessage(DynamicMessage && other) noexcept;
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicMessage & operator=(DynamicMessage && other) noexcept;
+
   RCLCPP_PUBLIC
   virtual ~DynamicMessage();
 
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_serialization_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
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_item_count() const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_member_id(size_t index) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_member_id(const std::string & name) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_array_index(size_t index) const;
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  get_array_index(const std::string & name) const;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  DynamicMessage
+  clone(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  init_from_type(
+    DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  init_from_type_shared(
+    DynamicMessageType & type, rcl_allocator_t allocator = rcl_get_default_allocator()) const;
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicMessage & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  loan_value(
+    rosidl_dynamic_typesupport_member_id_t id,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  loan_value(
+    const std::string & name,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  void
+  clear_all_values();
+
+  RCLCPP_PUBLIC
+  void
+  clear_nonkey_values();
+
+  RCLCPP_PUBLIC
+  void
+  clear_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  RCLCPP_PUBLIC
+  void
+  clear_value(const std::string & name);
+
+  RCLCPP_PUBLIC
+  void
+  clear_sequence();
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_sequence_data();
+
+  RCLCPP_PUBLIC
+  void
+  remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index);
+
+  RCLCPP_PUBLIC
+  bool
+  serialize(rcl_serialized_message_t & buffer);
+
+  RCLCPP_PUBLIC
+  bool
+  deserialize(rcl_serialized_message_t & buffer);
+
+
+  // MEMBER ACCESS TEMPLATES =======================================================================
+  /**
+   * Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
+   * https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
+   *
+   * Explicitly:
+   * - Basic types:        bool, byte, char
+   * - Float types:        float, double
+   * - Int types:          int8_t, int16_t, int32_t, int64_t
+   * - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
+   * - String types:       std::string, std::u16string
+   */
+
+  template<typename ValueT>
+  ValueT
+  get_value(rosidl_dynamic_typesupport_member_id_t id);
+
+  template<typename ValueT>
+  ValueT
+  get_value(const std::string & name);
+
+  template<typename ValueT>
+  void
+  set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value);
+
+  template<typename ValueT>
+  void
+  set_value(const std::string & name, ValueT value);
+
+  template<typename ValueT>
+  rosidl_dynamic_typesupport_member_id_t
+  insert_value(ValueT value);
+
+
+  // FIXED STRING MEMBER ACCESS ====================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length);
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_fixed_string_value(const std::string & name, size_t string_length);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_fixed_wstring_value(const std::string & name, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_string_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_string_value(const std::string & name, const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_wstring_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  void
+  set_fixed_wstring_value(
+    const std::string & name, const std::u16string value, size_t wstring_length);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_fixed_string_value(const std::string value, size_t string_length);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_fixed_wstring_value(const std::u16string value, size_t wstring_length);
+
+
+  // BOUNDED STRING MEMBER ACCESS ==================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  const std::string
+  get_bounded_string_value(const std::string & name, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  const std::u16string
+  get_bounded_wstring_value(const std::string & name, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_string_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_wstring_value(
+    rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  void
+  set_bounded_wstring_value(
+    const std::string & name, const std::u16string value, size_t wstring_bound);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_bounded_string_value(const std::string value, size_t string_bound);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound);
+
+
+  // NESTED MEMBER ACCESS ==========================================================================
+  RCLCPP_PUBLIC
+  DynamicMessage
+  get_nested_data(
+    rosidl_dynamic_typesupport_member_id_t id,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  get_nested_data(
+    const std::string & name,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_nested_data_shared(
+    rosidl_dynamic_typesupport_member_id_t id,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_nested_data_shared(
+    const std::string & name,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  void
+  set_nested_data(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  void
+  set_nested_data(const std::string & name, DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_nested_data_copy(const DynamicMessage & value);
+
+  RCLCPP_PUBLIC
+  rosidl_dynamic_typesupport_member_id_t
+  insert_nested_data(DynamicMessage & value);
+
 protected:
   // NOTE(methylDragon):
   // This is just here to extend the lifetime of the serialization support
@@ -62,6 +418,12 @@ private:
 
   RCLCPP_PUBLIC
   DynamicMessage();
+
+  RCLCPP_PUBLIC
+  bool
+  match_serialization_support_(
+    const DynamicSerializationSupport & serialization_support,
+    const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data);
 };
 
 }  // namespace dynamic_typesupport

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp

@@ -30,16 +30,143 @@ namespace rclcpp
 namespace dynamic_typesupport
 {
 
+class DynamicMessage;
+class DynamicMessageTypeBuilder;
+
 /// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t`
-/// STUBBED OUT
+/**
+ * This class:
+ * - Exposes getter methods for the struct
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the
+ *   passed `DynamicSerializationSupport`.
+ *   So it cannot outlive the `DynamicSerializationSupport`.
+ * - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t`
+ *   pointer must point to the same location in memory as the stored raw pointer!
+ *
+ * This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`,
+ * which can be constructed via `DynamicMessageTypeBuilder`, which maps to
+ * `rosidl_dynamic_typesupport_dynamic_type_builder_t`.
+ *
+ *  The usual method of obtaining a `DynamicMessageType` is through construction of
+ *  `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then
+ *  taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain
+ *  `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level
+ *  rosidl methods.
+ */
 class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
 
+  // CONSTRUCTION ==================================================================================
+  // Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the
+  // lifetime of the serialization support (if the constructor cannot otherwise get it from args).
+  //
+  // In cases where a dynamic type pointer is passed, the serialization support composed by
+  // the type should be the exact same object managed by the `DynamicSerializationSupport`,
+  // otherwise the lifetime management will not work properly.
+
+  /// Construct a new `DynamicMessageType` with the provided dynamic type builder,
+  /// using its allocator
+  RCLCPP_PUBLIC
+  explicit DynamicMessageType(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
+
+  /// Construct a new `DynamicMessageType` with the provided dynamic type builder and allocator
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder,
+    rcl_allocator_t allocator);
+
+  /// Assume ownership of struct
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type);
+
+  /// From description
+  RCLCPP_PUBLIC
+  DynamicMessageType(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Move constructor
+  RCLCPP_PUBLIC
+  DynamicMessageType(DynamicMessageType && other) noexcept;
+
+  /// Move assignment
+  RCLCPP_PUBLIC
+  DynamicMessageType & operator=(DynamicMessageType && other) noexcept;
+
   RCLCPP_PUBLIC
   virtual ~DynamicMessageType();
 
+  /// Swaps the serialization support if `serialization_support` is populated
+  /**
+   * The user can call this with another description to reconfigure the type without changing the
+   * serialization support
+   */
+  RCLCPP_PUBLIC
+  void
+  init_from_description(
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    rcl_allocator_t allocator = rcl_get_default_allocator(),
+    DynamicSerializationSupport::SharedPtr serialization_support = nullptr);
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_serialization_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
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+
+  // METHODS =======================================================================================
+  RCLCPP_PUBLIC
+  DynamicMessageType
+  clone(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::SharedPtr
+  clone_shared(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  bool
+  equals(const DynamicMessageType & other) const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage
+  build_dynamic_message(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<DynamicMessage>
+  build_dynamic_message_shared(rcl_allocator_t allocator = rcl_get_default_allocator());
+
 protected:
   // NOTE(methylDragon):
   // This is just here to extend the lifetime of the serialization support
@@ -56,6 +183,12 @@ private:
 
   RCLCPP_PUBLIC
   DynamicMessageType();
+
+  RCLCPP_PUBLIC
+  bool
+  match_serialization_support_(
+    const DynamicSerializationSupport & serialization_support,
+    const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type);
 };
 
 }  // namespace dynamic_typesupport

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp

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

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -38,16 +38,115 @@ namespace dynamic_typesupport
 {
 
 /// Utility wrapper class for `rosidl_message_type_support_t` containing managed
-/// STUBBED OUT
+/// instances of the typesupport handle impl.
+/**
+ *
+ * NOTE: This class is the recommended way to obtain the dynamic message type
+ *       support struct, instead of `rcl_dynamic_message_type_support_handle_init()`,
+ *       because this class will manage the lifetimes for you.
+ *
+ *       Do NOT call rcl_dynamic_message_type_support_handle_fini
+ *
+ * This class:
+ * - Exposes getter methods for the struct
+ * - Stores shared pointers to wrapper classes that expose the underlying
+ *   serialization support API
+ *
+ * Ownership:
+ * - This class, similarly to the `rosidl_dynamic_typesupport_serialization_support_t`, must outlive
+ *   all downstream usages of the serialization support.
+ */
 class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
 
+  // CONSTRUCTION ==================================================================================
+  /// From description
+  /// Does NOT take ownership of the description (copies instead.)
+  /// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    const std::string & serialization_library_name = "",
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// From description, for provided serialization support
+  /// Does NOT take ownership of the description (copies instead.)
+  /// Constructs type support top-down (calling `rosidl_dynamic_message_type_support_handle_init()`)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Assume ownership of managed types
+  /// Does NOT take ownership of the description (copies instead.)
+  ///
+  /// The serialization support used to construct all managed SharedPtrs must match.
+  /// The structure of the provided `description` must match the `dynamic_message_type`
+  /// The structure of the provided `dynamic_message_type` must match the `dynamic_message
+  ///
+  /// In this case, the user would have constructed the type support bototm-up (by creating the
+  /// respective dynamic members.)
+  RCLCPP_PUBLIC
+  DynamicMessageTypeSupport(
+    DynamicSerializationSupport::SharedPtr serialization_support,
+    DynamicMessageType::SharedPtr dynamic_message_type,
+    DynamicMessage::SharedPtr dynamic_message,
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
+
   RCLCPP_PUBLIC
   virtual ~DynamicMessageTypeSupport();
 
+
+  // GETTERS =======================================================================================
+  RCLCPP_PUBLIC
+  const std::string
+  get_serialization_library_identifier() const;
+
+  RCLCPP_PUBLIC
+  const rosidl_message_type_support_t &
+  get_const_rosidl_message_type_support();
+
+  RCLCPP_PUBLIC
+  const rosidl_message_type_support_t &
+  get_const_rosidl_message_type_support() const;
+
+  RCLCPP_PUBLIC
+  const rosidl_runtime_c__type_description__TypeDescription &
+  get_rosidl_runtime_c_type_description() const;
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::SharedPtr
+  get_shared_dynamic_serialization_support();
+
+  RCLCPP_PUBLIC
+  DynamicSerializationSupport::ConstSharedPtr
+  get_shared_dynamic_serialization_support() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::SharedPtr
+  get_shared_dynamic_message_type();
+
+  RCLCPP_PUBLIC
+  DynamicMessageType::ConstSharedPtr
+  get_shared_dynamic_message_type() const;
+
+  RCLCPP_PUBLIC
+  DynamicMessage::SharedPtr
+  get_shared_dynamic_message();
+
+  RCLCPP_PUBLIC
+  DynamicMessage::ConstSharedPtr
+  get_shared_dynamic_message() const;
+
 protected:
+  RCLCPP_PUBLIC
+  rosidl_message_type_support_t &
+  get_rosidl_message_type_support();
+
   DynamicSerializationSupport::SharedPtr serialization_support_;
   DynamicMessageType::SharedPtr dynamic_message_type_;
   DynamicMessage::SharedPtr dynamic_message_;

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp

@@ -31,22 +31,60 @@ namespace dynamic_typesupport
 {
 
 /// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t
+/**
+ * This class:
+ * - Exposes getter methods for the struct
+ * - Exposes the underlying serialization support API
+ *
+ * Ownership:
+ * - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive
+ *   all downstream usages of the serialization support.
+ */
 class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
 
+  // CONSTRUCTION ==================================================================================
   RCLCPP_PUBLIC
   explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator());
 
+  /// Get the rmw middleware implementation specific serialization support (configured by name)
   RCLCPP_PUBLIC
   DynamicSerializationSupport(
     const std::string & serialization_library_name,
     rcl_allocator_t allocator = rcl_get_default_allocator());
 
+  /// Assume ownership of struct
+  RCLCPP_PUBLIC
+  explicit DynamicSerializationSupport(
+    rosidl_dynamic_typesupport_serialization_support_t && rosidl_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_serialization_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;
+
 protected:
   rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_;
 

rclcpp/include/rclcpp/event_handler.hpp

@@ -233,8 +233,6 @@ protected:
   size_t wait_set_event_index_;
 };
 
-using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase;
-
 template<typename EventCallbackT, typename ParentHandleT>
 class EventHandler : public EventHandlerBase
 {
@@ -311,11 +309,6 @@ private:
   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/exceptions/exceptions.hpp

@@ -100,6 +100,15 @@ public:
   {}
 };
 
+class InvalidServiceTypeError : public std::runtime_error
+{
+public:
+  InvalidServiceTypeError()
+  : std::runtime_error("Service type is invalid.") {}
+  explicit InvalidServiceTypeError(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
 class UnimplementedError : public std::runtime_error
 {
 public:

rclcpp/include/rclcpp/executor.hpp

@@ -541,8 +541,9 @@ protected:
    *
    * \param[in] notify if true will execute a trigger that will wake up a waiting executor
    */
-  void
-  trigger_entity_recollect(bool notify);
+  RCLCPP_PUBLIC
+  virtual void
+  handle_updated_entities(bool notify);
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;

rclcpp/include/rclcpp/executor_options.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_
 #define RCLCPP__EXECUTOR_OPTIONS_HPP_
 
+#include <memory>
+
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -24,18 +26,30 @@
 namespace rclcpp
 {
 
+class ExecutorOptionsImplementation;
+
 /// Options to be passed to the executor constructor.
 struct ExecutorOptions
 {
-  ExecutorOptions()
-  : memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
-    context(rclcpp::contexts::get_global_default_context()),
-    max_conditions(0)
-  {}
+  RCLCPP_PUBLIC
+  ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  virtual ~ExecutorOptions();
+
+  RCLCPP_PUBLIC
+  ExecutorOptions(const ExecutorOptions &);
+
+  RCLCPP_PUBLIC
+  ExecutorOptions & operator=(const ExecutorOptions &);
 
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
   rclcpp::Context::SharedPtr context;
   size_t max_conditions;
+
+private:
+  /// Pointer to implementation
+  std::unique_ptr<ExecutorOptionsImplementation> impl_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -122,6 +122,14 @@ public:
   void
   clear_on_ready_callback() override;
 
+  /// Set a new callback to be called whenever this waitable is executed.
+  /**
+   * \param[in] on_execute_callback The new callback
+   */
+  RCLCPP_PUBLIC
+  void
+  set_execute_callback(std::function<void(void)> on_execute_callback);
+
   /// Remove a guard condition from being waited on.
   /**
    * \param[in] weak_guard_condition The guard condition to remove.
@@ -142,7 +150,10 @@ private:
   /// Callback to run when waitable executes
   std::function<void(void)> execute_callback_;
 
+  /// Mutex to procetect the guard conditions
   std::mutex guard_condition_mutex_;
+  /// Mutex to protect the execute callback
+  std::mutex execute_mutex_;
 
   std::function<void(size_t)> on_ready_callback_;
 

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -31,9 +31,15 @@ namespace executors
 /// Static executor implementation
 /**
  * This executor is a static version of the original single threaded executor.
- * It's static because it doesn't reconstruct the executable list for every iteration.
+ * It contains some performance optimization to avoid unnecessary reconstructions of
+ * the executable list for every iteration.
  * All nodes, callbackgroups, timers, subscriptions etc. are created before
  * spin() is called, and modified only when an entity is added/removed to/from a node.
+ * This executor is deprecated because these performance improvements have now been
+ * applied to all other executors.
+ * This executor is also considered unstable due to known bugs.
+ * See the unit-tests that are only applied to `StandardExecutors` for information
+ * on the known limitations.
  *
  * To run this executor instead of SingleThreadedExecutor replace:
  * rclcpp::executors::SingleThreadedExecutor exec;
@@ -44,7 +50,8 @@ namespace executors
  * exec.spin();
  * exec.remove_node(node);
  */
-class StaticSingleThreadedExecutor : public rclcpp::Executor
+class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor
+  : public rclcpp::Executor
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)

rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp

@@ -59,8 +59,6 @@ namespace executors
  */
 class EventsExecutor : public rclcpp::Executor
 {
-  friend class EventsExecutorEntitiesCollector;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
 
@@ -72,7 +70,7 @@ public:
    * \param[in] options Options used to configure the executor.
    */
   RCLCPP_PUBLIC
-  explicit EventsExecutor(
+  EventsExecutor(
     rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
       rclcpp::experimental::executors::SimpleEventsQueue>(),
     bool execute_timers_separate_thread = false,
@@ -128,87 +126,6 @@ public:
   void
   spin_all(std::chrono::nanoseconds max_duration) override;
 
-  /// Add a node to the executor.
-  /**
-   * \sa rclcpp::Executor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::EventsExecutor::add_node
-   */
-  RCLCPP_PUBLIC
-  void
-  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Remove a node from the executor.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Convenience function which takes Node and forwards NodeBaseInterface.
-  /**
-   * \sa rclcpp::Executor::remove_node
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
-
-  /// Add a callback group to an executor.
-  /**
-   * \sa rclcpp::Executor::add_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  add_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    bool notify = true) override;
-
-  /// Remove callback group from the executor
-  /**
-   * \sa rclcpp::Executor::remove_callback_group
-   */
-  RCLCPP_PUBLIC
-  void
-  remove_callback_group(
-    rclcpp::CallbackGroup::SharedPtr group_ptr,
-    bool notify = true) override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_all_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_all_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_manually_added_callback_groups()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_manually_added_callback_groups() override;
-
-  /// Get callback groups that belong to executor.
-  /**
-   * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::CallbackGroup::WeakPtr>
-  get_automatically_added_callback_groups_from_nodes() override;
-
 protected:
   /// Internal implementation of spin_once
   RCLCPP_PUBLIC
@@ -220,6 +137,11 @@ protected:
   void
   spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
 
+  /// Collect entities from callback groups and refresh the current collection with them
+  RCLCPP_PUBLIC
+  void
+  handle_updated_entities(bool notify) override;
+
 private:
   RCLCPP_DISABLE_COPY(EventsExecutor)
 
@@ -227,9 +149,9 @@ private:
   void
   execute_event(const ExecutorEvent & event);
 
-  /// Collect entities from callback groups and refresh the current collection with them
+  /// Rebuilds the executor's notify waitable, as we can't use the one built in the base class
   void
-  refresh_current_collection_from_callback_groups();
+  setup_notify_waitable();
 
   /// Refresh the current collection using the provided new_collection
   void
@@ -253,6 +175,11 @@ private:
   typename CollectionType::EntitySharedPtr
   retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
   {
+    // Note: we lock the mutex because we assume that you are trying to get an element from the
+    // current collection... If there will be a use-case to retrieve elements also from other
+    // collections, we can move the mutex back to the calling codes.
+    std::lock_guard<std::mutex> guard(mutex_);
+
     // Check if the entity_id is in the collection
     auto it = collection.find(entity_id);
     if (it == collection.end()) {
@@ -273,16 +200,6 @@ private:
   /// Queue where entities can push events
   rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
 
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
-  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
-
-  /// Mutex to protect the current_entities_collection_
-  std::recursive_mutex collection_mutex_;
-  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
-
-  /// Flag used to reduce the number of unnecessary waitable events
-  std::atomic<bool> notify_waitable_event_pushed_ {false};
-
   /// Timers manager used to track and/or execute associated timers
   std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
 };

rclcpp/include/rclcpp/generic_client.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <future>
 #include <string>
+#include <tuple>
 #include <vector>
 #include <utility>
 
@@ -35,8 +36,8 @@ namespace rclcpp
 class GenericClient : public ClientBase
 {
 public:
-  using Request = void *;   // Serialized data pointer of request message
-  using Response = void *;  // Serialized data pointer of response message
+  using Request = void *;   // Deserialized data pointer of request message
+  using Response = void *;  // Deserialized data pointer of response message
 
   using SharedResponse = std::shared_ptr<void>;
 
@@ -46,6 +47,8 @@ public:
   using Future = std::future<SharedResponse>;
   using SharedFuture = std::shared_future<SharedResponse>;
 
+  using CallbackType = std::function<void (SharedFuture)>;
+
   RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
 
   /// A convenient GenericClient::Future and request id pair.
@@ -76,6 +79,20 @@ public:
     ~FutureAndRequestId() = default;
   };
 
+  /// A convenient GenericClient::SharedFuture and request id pair.
+  /**
+   * Public members:
+   * - future: a std::shared_future<SharedResponse>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::shared_future provides.
+   */
+  struct SharedFutureAndRequestId
+    : detail::FutureAndRequestId<std::shared_future<SharedResponse>>
+  {
+    using detail::FutureAndRequestId<std::shared_future<SharedResponse>>::FutureAndRequestId;
+  };
+
   GenericClient(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
@@ -106,16 +123,16 @@ public:
    * If the future never completes,
    * e.g. the call to Executor::spin_until_future_complete() times out,
    * GenericClient::remove_pending_request() must be called to clean the client internal state.
-   * Not doing so will make the `Client` instance to use more memory each time a response is not
-   * received from the service server.
+   * Not doing so will make the `GenericClient` instance to use more memory each time a response is
+   * not received from the service server.
    *
    * ```cpp
-   * auto future = client->async_send_request(my_request);
+   * auto future = generic_client->async_send_request(my_request);
    * if (
    *   rclcpp::FutureReturnCode::TIMEOUT ==
    *   executor->spin_until_future_complete(future, timeout))
    * {
-   *   client->remove_pending_request(future);
+   *   generic_client->remove_pending_request(future);
    *   // handle timeout
    * } else {
    *   handle_response(future.get());
@@ -129,6 +146,45 @@ public:
   FutureAndRequestId
   async_send_request(const Request request);
 
+  /// Send a request to the service server and schedule a callback in the executor.
+  /**
+   * Similar to the previous overload, but a callback will automatically be called when a response
+   * is received.
+   *
+   * If the callback is never called, because we never got a reply for the service server,
+   * remove_pending_request() has to be called with the returned request id or
+   * prune_pending_requests().
+   * Not doing so will make the `GenericClient` instance use more memory each time a response is not
+   * received from the service server.
+   * In this case, it's convenient to setup a timer to cleanup the pending requests.
+   *
+   * \param[in] request request to be send.
+   * \param[in] cb callback that will be called when we get a response for this request.
+   * \return the request id representing the request just sent.
+   */
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureAndRequestId
+  async_send_request(const Request request, CallbackT && cb)
+  {
+    Promise promise;
+    auto shared_future = promise.get_future().share();
+    auto req_id = async_send_request_impl(
+      request,
+      std::make_tuple(
+        CallbackType{std::forward<CallbackT>(cb)},
+        shared_future,
+        std::move(promise)));
+    return SharedFutureAndRequestId{std::move(shared_future), req_id};
+  }
+
   /// Clean all pending requests older than a time_point.
   /**
    * \param[in] time_point Requests that were sent before this point are going to be removed.
@@ -149,15 +205,52 @@ public:
       pruned_requests);
   }
 
+  /// Clean all pending requests.
+  /**
+   * \return number of pending requests that were removed.
+   */
   RCLCPP_PUBLIC
   size_t
   prune_pending_requests();
 
+  /// Cleanup a pending request.
+  /**
+   * This notifies the client that we have waited long enough for a response from the server
+   * to come, we have given up and we are not waiting for a response anymore.
+   *
+   * Not calling this will make the client start using more memory for each request
+   * that never got a reply from the server.
+   *
+   * \param[in] request_id request id returned by async_send_request().
+   * \return true when a pending request was removed, false if not (e.g. a response was received).
+   */
   RCLCPP_PUBLIC
   bool
   remove_pending_request(
     int64_t request_id);
 
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const FutureAndRequestId & future);
+
+  /// Cleanup a pending request.
+  /**
+   * Convenient overload, same as:
+   *
+   * `GenericClient::remove_pending_request(this, future.request_id)`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  remove_pending_request(
+    const SharedFutureAndRequestId & future);
+
   /// Take the next response for this client.
   /**
    * \sa ClientBase::take_type_erased_response().
@@ -179,9 +272,12 @@ public:
   }
 
 protected:
+  using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
   using CallbackInfoVariant = std::variant<
-    std::promise<SharedResponse>>;  // Use variant for extension
+    std::promise<SharedResponse>,
+    CallbackTypeValueVariant>;  // Use variant for extension
 
+  RCLCPP_PUBLIC
   int64_t
   async_send_request_impl(
     const Request request,

rclcpp/include/rclcpp/generic_service.hpp

@@ -0,0 +1,308 @@
+// Copyright 2024 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.
+
+#ifndef RCLCPP__GENERIC_SERVICE_HPP_
+#define RCLCPP__GENERIC_SERVICE_HPP_
+
+#include <cstdlib>
+#include <functional>
+#include <memory>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <variant>
+
+#include "rclcpp/typesupport_helpers.hpp"
+
+#include "rosidl_runtime_c/service_type_support_struct.h"
+#include "rosidl_typesupport_introspection_cpp/identifier.hpp"
+#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp"
+
+#include "service.hpp"
+
+namespace rclcpp
+{
+class GenericService;
+
+class GenericServiceCallback
+{
+public:
+  using SharedRequest = std::shared_ptr<void>;
+  using SharedResponse = std::shared_ptr<void>;
+
+  GenericServiceCallback()
+  : callback_(std::monostate{})
+  {}
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT, can't satisfy both cpplint and uncrustify
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0>
+  void
+  set(CallbackT && callback)
+  {
+    if (!callback) {
+      throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr");
+    }
+    // Workaround Windows issue with std::bind
+    if constexpr (
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallback
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        SharedPtrDeferResponseCallbackWithServiceHandle
+      >::value)
+    {
+      callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else {
+      // the else clause is not needed, but anyways we should only be doing this instead
+      // of all the above workaround ...
+      callback_ = std::forward<CallbackT>(callback);
+    }
+  }
+
+  SharedResponse
+  dispatch(
+    const std::shared_ptr<rclcpp::GenericService> & service_handle,
+    const std::shared_ptr<rmw_request_id_t> & request_header,
+    SharedRequest request,
+    SharedRequest response)
+  {
+    TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
+    if (std::holds_alternative<std::monostate>(callback_)) {
+      // TODO(ivanpauno): Remove the set method, and force the users of this class
+      // to pass a callback at construnciton.
+      throw std::runtime_error{"unexpected request without any callback set"};
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
+      cb(request_header, std::move(request));
+      return nullptr;
+    }
+    if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
+      const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
+      cb(service_handle, request_header, std::move(request));
+      return nullptr;
+    }
+
+    if (std::holds_alternative<SharedPtrCallback>(callback_)) {
+      (void)request_header;
+      const auto & cb = std::get<SharedPtrCallback>(callback_);
+      cb(std::move(request), std::move(response));
+    } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
+      const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
+      cb(request_header, std::move(request), std::move(response));
+    }
+    TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
+    return response;
+  }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    std::visit(
+      [this](auto && arg) {
+        if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
+          char * symbol = tracetools::get_symbol(arg);
+          TRACETOOLS_DO_TRACEPOINT(
+            rclcpp_callback_register,
+            static_cast<const void *>(this),
+            symbol);
+          std::free(symbol);
+        }
+      }, callback_);
+#endif  // TRACETOOLS_DISABLED
+  }
+
+private:
+  using SharedPtrCallback = std::function<void (SharedRequest, SharedResponse)>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest,
+      SharedResponse
+    )>;
+  using SharedPtrDeferResponseCallback = std::function<
+    void (
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest
+    )>;
+  using SharedPtrDeferResponseCallbackWithServiceHandle = std::function<
+    void (
+      std::shared_ptr<rclcpp::GenericService>,
+      std::shared_ptr<rmw_request_id_t>,
+      SharedRequest
+    )>;
+
+  std::variant<
+    std::monostate,
+    SharedPtrCallback,
+    SharedPtrWithRequestHeaderCallback,
+    SharedPtrDeferResponseCallback,
+    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
+};
+
+class GenericService
+  : public ServiceBase,
+  public std::enable_shared_from_this<GenericService>
+{
+public:
+  using Request = void *;  // Serialized/Deserialized data pointer of request message
+  using Response = void *;  // Serialized/Deserialized data pointer of response message
+  using SharedRequest = std::shared_ptr<void>;
+  using SharedResponse = std::shared_ptr<void>;
+  using CallbackType = std::function<void (const SharedRequest, SharedResponse)>;
+
+  using CallbackWithHeaderType =
+    std::function<void (const std::shared_ptr<rmw_request_id_t>,
+      const SharedRequest,
+      SharedResponse)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericService)
+
+  /// Default constructor.
+  /**
+   * The constructor for a Service is almost never called directly.
+   * Instead, services should be instantiated through the function
+   * rclcpp::create_service().
+   *
+   * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
+   * \param[in] service_name Name of the topic to publish to.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool".
+   * \param[in] any_callback User defined callback to call when a client request is received.
+   * \param[in] service_options options for the service.
+   */
+  RCLCPP_PUBLIC
+  GenericService(
+    std::shared_ptr<rcl_node_t> node_handle,
+    const std::string & service_name,
+    const std::string & service_type,
+    GenericServiceCallback any_callback,
+    rcl_service_options_t & service_options);
+
+  GenericService() = delete;
+
+  RCLCPP_PUBLIC
+  virtual ~GenericService() {}
+
+  /// Take the next request from the service.
+  /**
+   * \sa ServiceBase::take_type_erased_request().
+   *
+   * \param[out] request_out The reference to a service deserialized request object
+   *   into which the middleware will copy the taken request.
+   * \param[out] request_id_out The output id for the request which can be used
+   *   to associate response with this request in the future.
+   * \returns true if the request was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl calls fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_request(SharedRequest request_out, rmw_request_id_t & request_id_out);
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  create_request() override;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<void>
+  create_response();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override;
+
+  RCLCPP_PUBLIC
+  void
+  handle_request(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> request) override;
+
+  RCLCPP_PUBLIC
+  void
+  send_response(rmw_request_id_t & req_id, SharedResponse & response);
+
+private:
+  RCLCPP_DISABLE_COPY(GenericService)
+
+  GenericServiceCallback any_callback_;
+
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * request_members_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_;
+};
+
+}  // namespace rclcpp
+#endif  // RCLCPP__GENERIC_SERVICE_HPP_

rclcpp/include/rclcpp/loaned_message.hpp

@@ -31,21 +31,20 @@ namespace rclcpp
 template<typename MessageT, typename AllocatorT = std::allocator<void>>
 class LoanedMessage
 {
+public:
   using MessageAllocatorTraits = rclcpp::allocator::AllocRebind<MessageT, AllocatorT>;
   using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
 
-public:
   /// Constructor of the LoanedMessage class.
   /**
    * The constructor of this class allocates memory for a given message type
    * and associates this with a given publisher.
    *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
+   * The underlying middleware is queried to determine whether it is able to allocate the
+   * appropriate memory for this message type or not.
+   * In the case that the middleware cannot loan messages, the passed in allocator instance
+   * is used to allocate the message within the scope of this class.
+   * Otherwise, the allocator is ignored and the allocation is solely performed
    * in the underlying middleware with its appropriate allocation strategy.
    * The need for this arises as the user code can be written explicitly targeting a middleware
    * capable of loaning messages.
@@ -53,12 +52,12 @@ public:
    * a middleware which doesn't support message loaning in which case the allocator will be used.
    *
    * \param[in] pub rclcpp::Publisher instance to which the memory belongs
-   * \param[in] allocator Allocator instance in case middleware can not allocate messages
+   * \param[in] allocator Allocator instance in case middleware cannot allocate messages
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */
   LoanedMessage(
     const rclcpp::PublisherBase & pub,
-    std::allocator<MessageT> allocator)
+    MessageAllocator allocator)
   : pub_(pub),
     message_(nullptr),
     message_allocator_(std::move(allocator))
@@ -82,36 +81,6 @@ public:
     }
   }
 
-  /// Constructor of the LoanedMessage class.
-  /**
-   * The constructor of this class allocates memory for a given message type
-   * and associates this with a given publisher.
-   *
-   * Given the publisher instance, a case differentiation is being performaned
-   * which decides whether the underlying middleware is able to allocate the appropriate
-   * memory for this message type or not.
-   * In the case that the middleware can not loan messages, the passed in allocator instance
-   * is being used to allocate the message within the scope of this class.
-   * Otherwise, the allocator is being ignored and the allocation is solely performaned
-   * in the underlying middleware with its appropriate allocation strategy.
-   * The need for this arises as the user code can be written explicitly targeting a middleware
-   * capable of loaning messages.
-   * However, this user code is ought to be usable even when dynamically linked against
-   * a middleware which doesn't support message loaning in which case the allocator will be used.
-   *
-   * \param[in] pub rclcpp::Publisher instance to which the memory belongs
-   * \param[in] allocator Allocator instance in case middleware can not allocate messages
-   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
-   */
-  [[
-    deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator")
-  ]]
-  LoanedMessage(
-    const rclcpp::PublisherBase * pub,
-    std::shared_ptr<std::allocator<MessageT>> allocator)
-  : LoanedMessage(*pub, *allocator)
-  {}
-
   /// Move semantic for RVO
   LoanedMessage(LoanedMessage<MessageT> && other)
   : pub_(std::move(other.pub_)),

rclcpp/include/rclcpp/logger.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__LOGGER_HPP_
 #define RCLCPP__LOGGER_HPP_
 
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <utility>
@@ -77,6 +78,34 @@ RCLCPP_PUBLIC
 Logger
 get_node_logger(const rcl_node_t * node);
 
+// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted).
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+/// Get the current logging directory.
+/**
+ * For more details of how the logging directory is determined,
+ * see rcl_logging_get_logging_directory().
+ *
+ * \returns the logging directory being used.
+ * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
+ */
+[[deprecated("use rclcpp::get_log_directory instead of rclcpp::get_logging_directory")]]
+RCLCPP_PUBLIC
+rcpputils::fs::path
+get_logging_directory();
+
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+
 /// Get the current logging directory.
 /**
  * For more details of how the logging directory is determined,
@@ -86,8 +115,8 @@ get_node_logger(const rcl_node_t * node);
  * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
  */
 RCLCPP_PUBLIC
-rcpputils::fs::path
-get_logging_directory();
+std::filesystem::path
+get_log_directory();
 
 class Logger
 {

rclcpp/include/rclcpp/node.hpp

@@ -44,6 +44,7 @@
 #include "rclcpp/event.hpp"
 #include "rclcpp/generic_client.hpp"
 #include "rclcpp/generic_publisher.hpp"
+#include "rclcpp/generic_service.hpp"
 #include "rclcpp/generic_subscription.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
@@ -257,22 +258,6 @@ public:
     CallbackT callback,
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create and return a Client.
-  /**
-   * \param[in] service_name The topic to service on.
-   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
-   * \param[in] group Callback group to call the service.
-   * \return Shared pointer to the created client.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename ServiceT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  typename rclcpp::Client<ServiceT>::SharedPtr
-  create_client(
-    const std::string & service_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
-
   /// Create and return a Client.
   /**
    * \param[in] service_name The name on which the service is accessible.
@@ -287,24 +272,6 @@ public:
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Create and return a Service.
-  /**
-   * \param[in] service_name The topic to service on.
-   * \param[in] callback User-defined callback function.
-   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
-   * \param[in] group Callback group to call the service.
-   * \return Shared pointer to the created service.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename ServiceT, typename CallbackT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  typename rclcpp::Service<ServiceT>::SharedPtr
-  create_service(
-    const std::string & service_name,
-    CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
-
   /// Create and return a Service.
   /**
    * \param[in] service_name The topic to service on.
@@ -337,6 +304,24 @@ public:
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create and return a GenericService.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool"
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos Quality of service profile for the service.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
+   */
+  template<typename CallbackT>
+  typename rclcpp::GenericService::SharedPtr
+  create_generic_service(
+    const std::string & service_name,
+    const std::string & service_type,
+    CallbackT && callback,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
   /// Create and return a GenericPublisher.
   /**
    * The returned pointer will never be empty, but this function can throw various exceptions, for
@@ -1015,8 +1000,6 @@ public:
     rclcpp::node_interfaces::OnSetParametersCallbackHandle;
   using OnSetParametersCallbackType =
     rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
-  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
-    OnSetParametersCallbackType;
 
   using PostSetParametersCallbackHandle =
     rclcpp::node_interfaces::PostSetParametersCallbackHandle;
@@ -1447,7 +1430,7 @@ public:
   rclcpp::Clock::ConstSharedPtr
   get_clock() const;
 
-  /// Returns current time from the time source specified by clock_type.
+  /// Returns current time from the node clock.
   /**
    * \sa rclcpp::Clock::now
    */

rclcpp/include/rclcpp/node_impl.hpp

@@ -40,6 +40,7 @@
 #include "rclcpp/create_generic_subscription.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
+#include "rclcpp/create_generic_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/create_timer.hpp"
 #include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
@@ -154,22 +155,6 @@ Node::create_client(
     group);
 }
 
-template<typename ServiceT>
-typename Client<ServiceT>::SharedPtr
-Node::create_client(
-  const std::string & service_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
-{
-  return rclcpp::create_client<ServiceT>(
-    node_base_,
-    node_graph_,
-    node_services_,
-    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
-    qos_profile,
-    group);
-}
-
 template<typename ServiceT, typename CallbackT>
 typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
@@ -187,20 +172,22 @@ Node::create_service(
     group);
 }
 
-template<typename ServiceT, typename CallbackT>
-typename rclcpp::Service<ServiceT>::SharedPtr
-Node::create_service(
+template<typename CallbackT>
+typename rclcpp::GenericService::SharedPtr
+Node::create_generic_service(
   const std::string & service_name,
+  const std::string & service_type,
   CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
+  const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
+  return rclcpp::create_generic_service<CallbackT>(
     node_base_,
     node_services_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    service_type,
     std::forward<CallbackT>(callback),
-    qos_profile,
+    qos,
     group);
 }
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -52,8 +52,6 @@ struct OnSetParametersCallbackHandle
     std::function<
     rcl_interfaces::msg::SetParametersResult(
       const std::vector<rclcpp::Parameter> &)>;
-  using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
-    OnSetParametersCallbackType;
 
   OnSetParametersCallbackType callback;
 };

rclcpp/include/rclcpp/parameter_client.hpp

@@ -52,37 +52,6 @@ class AsyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
-  /// Create an async parameters client.
-  /**
-   * \param[in] node_base_interface The node base interface of the corresponding node.
-   * \param[in] node_topics_interface Node topic base interface.
-   * \param[in] node_graph_interface The node graph interface of the corresponding node.
-   * \param[in] node_services_interface Node service interface.
-   * \param[in] remote_node_name Name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  AsyncParametersClient(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node_base_interface,
-      node_topics_interface,
-      node_graph_interface,
-      node_services_interface,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /// Create an async parameters client.
   /**
    * \param[in] node_base_interface The node base interface of the corresponding node.
@@ -103,31 +72,6 @@ public:
     const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  /// Constructor
-  /**
-   * \param[in] node The async parameters client will be added to this node.
-   * \param[in] remote_node_name name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  AsyncParametersClient(
-    const std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /**
    * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
@@ -150,31 +94,6 @@ public:
       group)
   {}
 
-  /// Constructor
-  /**
-   * \param[in] node The async parameters client will be added to this node.
-   * \param[in] remote_node_name Name of the remote node
-   * \param[in] qos_profile The rmw qos profile to use to subscribe
-   * \param[in] group (optional) The async parameter client will be added to this callback group.
-   * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t
-   */
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  AsyncParametersClient(
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr)
-  : AsyncParametersClient(
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)),
-      group)
-  {}
-
   /**
    * \param[in] node The async parameters client will be added to this node.
    * \param[in] remote_node_name (optional) name of the remote node
@@ -383,19 +302,6 @@ class SyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   explicit SyncParametersClient(
     std::shared_ptr<NodeT> node,
@@ -408,23 +314,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    std::shared_ptr<NodeT> node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      executor,
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
@@ -441,19 +330,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
-      node,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   explicit SyncParametersClient(
     NodeT * node,
@@ -466,23 +342,6 @@ public:
       qos_profile)
   {}
 
-  template<typename NodeT>
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    NodeT * node,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : SyncParametersClient(
-      executor,
-      node->get_node_base_interface(),
-      node->get_node_topics_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   template<typename NodeT>
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,
@@ -499,28 +358,6 @@ public:
       qos_profile)
   {}
 
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  SyncParametersClient(
-    rclcpp::Executor::SharedPtr executor,
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
-    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
-    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
-    const std::string & remote_node_name,
-    const rmw_qos_profile_t & qos_profile)
-  : executor_(executor), node_base_interface_(node_base_interface)
-  {
-    async_parameters_client_ =
-      std::make_shared<AsyncParametersClient>(
-      node_base_interface,
-      node_topics_interface,
-      node_graph_interface,
-      node_services_interface,
-      remote_node_name,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)));
-  }
-
   RCLCPP_PUBLIC
   SyncParametersClient(
     rclcpp::Executor::SharedPtr executor,

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -67,19 +67,23 @@ struct ParameterEventCallbackHandle
  * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use
  * to create any required subscriptions:
  *
- *   auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
+ * ```cpp
+ * auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
+ * ```
  *
  * Next, you can supply a callback to the add_parameter_callback method, as follows:
  *
- *   auto cb1 = [&node](const rclcpp::Parameter & p) {
- *      RCLCPP_INFO(
- *        node->get_logger(),
- *        "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
- *        p.get_name().c_str(),
- *        p.get_type_name().c_str(),
- *        p.as_int());
- *   };
- *   auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
+ * ```cpp
+ * auto cb1 = [&node](const rclcpp::Parameter & p) {
+ *    RCLCPP_INFO(
+ *      node->get_logger(),
+ *      "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
+ *      p.get_name().c_str(),
+ *      p.get_type_name().c_str(),
+ *      p.as_int());
+ * };
+ * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
+ * ```
  *
  * In this case, we didn't supply a node name (the third, optional, parameter) so the
  * default will be to monitor for changes to the "an_int_param" parameter associated with
@@ -92,16 +96,18 @@ struct ParameterEventCallbackHandle
  * You may also monitor for changes to parameters in other nodes by supplying the node
  * name to add_parameter_callback:
  *
- *   auto cb2 = [&node](const rclcpp::Parameter & p) {
- *       RCLCPP_INFO(
- *         node->get_logger(),
- *         "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *         p.get_name().c_str(),
- *         p.get_type_name().c_str(),
- *         p.as_string().c_str());
- *     };
- *   auto handle2 = param_handler->add_parameter_callback(
- *     "some_remote_param_name", cb2, "some_remote_node_name");
+ * ```cpp
+ * auto cb2 = [&node](const rclcpp::Parameter & p) {
+ *     RCLCPP_INFO(
+ *       node->get_logger(),
+ *       "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *       p.get_name().c_str(),
+ *       p.get_type_name().c_str(),
+ *       p.as_string().c_str());
+ *   };
+ * auto handle2 = param_handler->add_parameter_callback(
+ *   "some_remote_param_name", cb2, "some_remote_node_name");
+ * ```
  *
  * In this case, the callback will be invoked whenever "some_remote_param_name" changes
  * on remote node "some_remote_node_name".
@@ -109,7 +115,9 @@ struct ParameterEventCallbackHandle
  * To remove a parameter callback, reset the callback handle smart pointer or call
  * remove_parameter_callback, passing the handle returned from add_parameter_callback:
  *
- *   param_handler->remove_parameter_callback(handle2);
+ * ```cpp
+ * param_handler->remove_parameter_callback(handle2);
+ * ```
  *
  * You can also monitor for *all* parameter changes, using add_parameter_event_callback.
  * In this case, the callback will be invoked whenever any parameter changes in the system.
@@ -117,40 +125,42 @@ struct ParameterEventCallbackHandle
  * is convenient to use a regular expression on the node names or namespaces of interest.
  * For example:
  *
- *   auto cb3 =
- *     [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
- *       // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
- *       // to our own node ("this_node")
- *       std::regex re("(/a_namespace/.*)|(/this_node)");
- *       if (regex_match(event.node, re)) {
- *         // Now that we know the event matches the regular expression we scanned for, we can
- *         // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
- *         rclcpp::Parameter p;
- *         if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event(
- *             event, p, remote_param_name, fqn))
- *         {
- *           RCLCPP_INFO(
- *             node->get_logger(),
- *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *             p.get_name().c_str(),
- *             p.get_type_name().c_str(),
- *             p.as_string().c_str());
- *         }
- *
- *         // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
- *         // in on this event
- *         auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(event);
- *         for (auto & p : params) {
- *           RCLCPP_INFO(
- *             node->get_logger(),
- *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
- *             p.get_name().c_str(),
- *             p.get_type_name().c_str(),
- *             p.value_to_string().c_str());
- *         }
+ * ```cpp
+ * auto cb3 =
+ *   [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
+ *     // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
+ *     // to our own node ("this_node")
+ *     std::regex re("(/a_namespace/.*)|(/this_node)");
+ *     if (regex_match(event.node, re)) {
+ *       // Now that we know the event matches the regular expression we scanned for, we can
+ *       // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
+ *       rclcpp::Parameter p;
+ *       if (rclcpp::ParameterEventHandler::get_parameter_from_event(
+ *           event, p, remote_param_name, fqn))
+ *       {
+ *         RCLCPP_INFO(
+ *           node->get_logger(),
+ *           "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *           p.get_name().c_str(),
+ *           p.get_type_name().c_str(),
+ *           p.as_string().c_str());
  *       }
- *     };
- *   auto handle3 = param_handler->add_parameter_event_callback(cb3);
+ *
+ *       // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
+ *       // in on this event
+ *       auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event);
+ *       for (auto & p : params) {
+ *         RCLCPP_INFO(
+ *           node->get_logger(),
+ *           "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *           p.get_name().c_str(),
+ *           p.get_type_name().c_str(),
+ *           p.value_to_string().c_str());
+ *       }
+ *     }
+ *   };
+ * auto handle3 = param_handler->add_parameter_event_callback(cb3);
+ * ```
  *
  * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added,
  * the callbacks are invoked last-in, first-called order (LIFO).
@@ -160,7 +170,9 @@ struct ParameterEventCallbackHandle
  *
  * To remove a parameter event callback, reset the callback smart pointer or use:
  *
- *   param_handler->remove_event_parameter_callback(handle3);
+ * ```cpp
+ * param_handler->remove_event_parameter_callback(handle3);
+ * ```
  */
 class ParameterEventHandler
 {

rclcpp/include/rclcpp/parameter_service.hpp

@@ -40,20 +40,6 @@ class ParameterService
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(ParameterService)
 
-  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
-  RCLCPP_PUBLIC
-  ParameterService(
-    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-    rclcpp::node_interfaces::NodeParametersInterface * node_params,
-    const rmw_qos_profile_t & qos_profile)
-  : ParameterService(
-      node_base,
-      node_services,
-      node_params,
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)))
-  {}
-
   RCLCPP_PUBLIC
   ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,

rclcpp/include/rclcpp/parameter_value.hpp

@@ -359,7 +359,7 @@ private:
 /// Return the value of a parameter as a string
 RCLCPP_PUBLIC
 std::string
-to_string(const ParameterValue & type);
+to_string(const ParameterValue & value);
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/publisher.hpp

@@ -96,22 +96,6 @@ public:
   using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
   using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
 
-  using MessageAllocatorTraits
-  [[deprecated("use PublishedTypeAllocatorTraits")]] =
-    PublishedTypeAllocatorTraits;
-  using MessageAllocator
-  [[deprecated("use PublishedTypeAllocator")]] =
-    PublishedTypeAllocator;
-  using MessageDeleter
-  [[deprecated("use PublishedTypeDeleter")]] =
-    PublishedTypeDeleter;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<PublishedType, PublishedTypeDeleter>")]] =
-    std::unique_ptr<PublishedType, PublishedTypeDeleter>;
-  using MessageSharedPtr
-  [[deprecated("use std::shared_ptr<const PublishedType>")]] =
-    std::shared_ptr<const PublishedType>;
-
   using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
     ROSMessageType,
     ROSMessageTypeAllocator,
@@ -128,8 +112,8 @@ public:
    *
    * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] topic Name of the topic to publish to.
-   * \param[in] qos QoS profile for Subcription.
-   * \param[in] options Options for the subscription.
+   * \param[in] qos QoS profile for the publisher.
+   * \param[in] options Options for the publisher.
    */
   Publisher(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
@@ -162,8 +146,7 @@ public:
     const rclcpp::QoS & qos,
     const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
   {
-    // Topic is unused for now.
-    (void)topic;
+    (void)qos;
     (void)options;
 
     // If needed, setup intra process communication.
@@ -171,22 +154,26 @@ public:
       auto context = node_base->get_context();
       // Get the intra process manager instance for this context.
       auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
-      // Register the publisher with the intra process manager.
-      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
+      // Check if the QoS is compatible with intra-process.
+      auto qos_profile = get_actual_qos();
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic +
+                "' allowed only with keep last history qos policy");
       }
-      if (qos.depth() == 0) {
+      if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with a zero qos history depth value");
+                "intraprocess communication on topic '" + topic +
+                "' is not allowed with a zero qos history depth value");
       }
-      if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
+      if (qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
         buffer_ = rclcpp::experimental::create_intra_process_buffer<
           ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>(
           rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type),
-          qos,
+          qos_profile,
           std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
       }
+      // Register the publisher with the intra process manager.
       uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_);
       this->setup_intra_process(
         intra_process_publisher_id,
@@ -203,7 +190,7 @@ public:
    * the loaned message will be directly allocated in the middleware.
    * If not, the message allocator of this rclcpp::Publisher instance is being used.
    *
-   * With a call to \sa `publish` the LoanedMessage instance is being returned to the middleware
+   * With a call to `publish` the LoanedMessage instance is being returned to the middleware
    * or free'd accordingly to the allocator.
    * If the message is not being published but processed differently, the destructor of this
    * class will either return the message to the middleware or deallocate it via the internal
@@ -248,8 +235,12 @@ public:
     // interprocess publish, resulting in lower publish-to-subscribe latency.
     // It's not possible to do that with an unique_ptr,
     // as do_intra_process_publish takes the ownership of the message.
+
+    // When durability is set to TransientLocal (i.e. there is a buffer),
+    // inter process publish should always take place to ensure
+    // late joiners receive past data.
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_subscription_count() > get_intra_process_subscription_count() || buffer_;
 
     if (inter_process_publish_needed) {
       auto shared_msg =
@@ -326,8 +317,11 @@ public:
       return;
     }
 
+    // When durability is set to TransientLocal (i.e. there is a buffer),
+    // inter process publish should always take place to ensure
+    // late joiners receive past data.
     bool inter_process_publish_needed =
-      get_subscription_count() > get_intra_process_subscription_count();
+      get_subscription_count() > get_intra_process_subscription_count() || buffer_;
 
     if (inter_process_publish_needed) {
       auto ros_msg_ptr = std::make_shared<ROSMessageType>();
@@ -423,13 +417,6 @@ public:
     }
   }
 
-  [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]]
-  std::shared_ptr<PublishedTypeAllocator>
-  get_allocator() const
-  {
-    return std::make_shared<PublishedTypeAllocator>(published_type_allocator_);
-  }
-
   PublishedTypeAllocator
   get_published_type_allocator() const
   {

rclcpp/include/rclcpp/rate.hpp

@@ -39,10 +39,6 @@ public:
   RCLCPP_PUBLIC
   virtual bool sleep() = 0;
 
-  [[deprecated("use get_type() instead")]]
-  RCLCPP_PUBLIC
-  virtual bool is_steady() const = 0;
-
   RCLCPP_PUBLIC
   virtual rcl_clock_type_t get_type() const = 0;
 
@@ -54,82 +50,6 @@ using std::chrono::duration;
 using std::chrono::duration_cast;
 using std::chrono::nanoseconds;
 
-template<class Clock = std::chrono::high_resolution_clock>
-class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
-
-  explicit GenericRate(double rate)
-  : period_(duration_cast<nanoseconds>(duration<double>(1.0 / rate))), last_interval_(Clock::now())
-  {}
-  explicit GenericRate(std::chrono::nanoseconds period)
-  : period_(period), last_interval_(Clock::now())
-  {}
-
-  virtual bool
-  sleep()
-  {
-    // Time coming into sleep
-    auto now = Clock::now();
-    // Time of next interval
-    auto next_interval = last_interval_ + period_;
-    // Detect backwards time flow
-    if (now < last_interval_) {
-      // Best thing to do is to set the next_interval to now + period
-      next_interval = now + period_;
-    }
-    // Calculate the time to sleep
-    auto time_to_sleep = next_interval - now;
-    // Update the interval
-    last_interval_ += period_;
-    // If the time_to_sleep is negative or zero, don't sleep
-    if (time_to_sleep <= std::chrono::seconds(0)) {
-      // If an entire cycle was missed then reset next interval.
-      // This might happen if the loop took more than a cycle.
-      // Or if time jumps forward.
-      if (now > next_interval + period_) {
-        last_interval_ = now + period_;
-      }
-      // Either way do not sleep and return false
-      return false;
-    }
-    // Sleep (will get interrupted by ctrl-c, may not sleep full time)
-    rclcpp::sleep_for(time_to_sleep);
-    return true;
-  }
-
-  [[deprecated("use get_type() instead")]]
-  virtual bool
-  is_steady() const
-  {
-    return Clock::is_steady;
-  }
-
-  virtual rcl_clock_type_t get_type() const
-  {
-    return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME;
-  }
-
-  virtual void
-  reset()
-  {
-    last_interval_ = Clock::now();
-  }
-
-  std::chrono::nanoseconds period() const
-  {
-    return period_;
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(GenericRate)
-
-  std::chrono::nanoseconds period_;
-  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
-  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
-};
-
 class Rate : public RateBase
 {
 public:
@@ -149,11 +69,6 @@ public:
   virtual bool
   sleep();
 
-  [[deprecated("use get_type() instead")]]
-  RCLCPP_PUBLIC
-  virtual bool
-  is_steady() const;
-
   RCLCPP_PUBLIC
   virtual rcl_clock_type_t
   get_type() const;

rclcpp/include/rclcpp/rclcpp.hpp

@@ -133,6 +133,9 @@
  * - Dynamic typesupport
  *   - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
  *   - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
+ * - Dynamic subscription
+ *   - rclcpp::DynamicSubscription
+ *   - rclcpp/dynamic_subscription.hpp
  * - Generic publisher
  *   - rclcpp::Node::create_generic_publisher()
  *   - rclcpp::GenericPublisher
@@ -184,4 +187,6 @@
 #include "rclcpp/waitable.hpp"
 #include "rclcpp/wait_set.hpp"
 
+#include "rclcpp/dynamic_subscription.hpp"
+
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -307,7 +307,7 @@ public:
    * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] service_name Name of the topic to publish to.
    * \param[in] any_callback User defined callback to call when a client request is received.
-   * \param[in] service_options options for the subscription.
+   * \param[in] service_options options for the service.
    */
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
@@ -499,7 +499,7 @@ public:
     }
   }
 
-  /// Configure client introspection.
+  /// Configure service 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

rclcpp/include/rclcpp/subscription.hpp

@@ -90,18 +90,6 @@ public:
   using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
   using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
 
-  using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] =
-    ROSMessageTypeAllocatorTraits;
-  using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] =
-    ROSMessageTypeAllocator;
-  using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] =
-    ROSMessageTypeDeleter;
-
-  using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr<const ROSMessageType>;
-  using MessageUniquePtr
-  [[deprecated("use std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter> instead")]] =
-    std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>;
-
 private:
   using SubscriptionTopicStatisticsSharedPtr =
     std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;
@@ -159,11 +147,13 @@ public:
       auto qos_profile = get_actual_qos();
       if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' allowed only with keep last history qos policy");
       }
       if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with 0 depth qos policy");
+                "intraprocess communication on topic '" + topic_name +
+                "' is not allowed with 0 depth qos policy");
       }
 
       using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<

rclcpp/include/rclcpp/subscription_base.hpp

@@ -644,6 +644,10 @@ protected:
 
   rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
 
+  // TODO(methylDragon): Remove if we don't need this
+  // rclcpp::node_interfaces::NodeGraphInterface * const node_graph_;
+  // rclcpp::node_interfaces::NodeServicesInterface * const node_services_;
+
   std::shared_ptr<rcl_node_t> node_handle_;
 
   std::recursive_mutex callback_mutex_;

rclcpp/include/rclcpp/subscription_options.hpp

@@ -80,7 +80,8 @@ struct SubscriptionOptionsBase
 
     // An optional QoS which can provide topic_statistics with a stable QoS separate from
     // the subscription's current QoS settings which could be unstable.
-    rclcpp::QoS qos = SystemDefaultsQoS();
+    // Explicitly set the enough depth to avoid missing the statistics messages.
+    rclcpp::QoS qos = SystemDefaultsQoS().keep_last(10);
   };
 
   TopicStatisticsOptions topic_stats_options;

rclcpp/include/rclcpp/time.hpp

@@ -49,6 +49,7 @@ public:
   /**
    * \param nanoseconds since time epoch
    * \param clock_type clock type
+   * \throws std::runtime_error if nanoseconds are negative
    */
   RCLCPP_PUBLIC
   explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -172,12 +172,11 @@ private:
   {
     auto received_message_age = std::make_unique<ReceivedMessageAge>();
     received_message_age->Start();
-    subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
-
     auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
     received_message_period->Start();
     {
       std::lock_guard<std::mutex> lock(mutex_);
+      subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
       subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
     }
 

rclcpp/include/rclcpp/typesupport_helpers.hpp

@@ -38,25 +38,6 @@ RCLCPP_PUBLIC
 std::shared_ptr<rcpputils::SharedLibrary>
 get_typesupport_library(const std::string & type, const std::string & typesupport_identifier);
 
-/// Extract the type support handle from the library.
-/**
- * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
- *
- * \deprecated Use get_message_typesupport_handle() instead
- *
- * \param[in] type The topic type, e.g. "std_msgs/msg/String"
- * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
- * \param[in] library The shared type support library
- * \return A type support handle
- */
-[[deprecated("Use `get_message_typesupport_handle` instead")]]
-RCLCPP_PUBLIC
-const rosidl_message_type_support_t *
-get_typesupport_handle(
-  const std::string & type,
-  const std::string & typesupport_identifier,
-  rcpputils::SharedLibrary & library);
-
 /// Extract the message type support handle from the library.
 /**
  * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.

rclcpp/include/rclcpp/waitable.hpp

@@ -101,23 +101,6 @@ public:
   size_t
   get_number_of_ready_guard_conditions();
 
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Woverloaded-virtual"
-#endif
-  /// Deprecated.
-  /**
-   * \sa add_to_wait_set(rcl_wait_set_t &)
-   */
-  [[deprecated("Use add_to_wait_set(rcl_wait_set_t & wait_set) signature")]]
-  RCLCPP_PUBLIC
-  virtual
-  void
-  add_to_wait_set(rcl_wait_set_t * wait_set);
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
-
   /// Add the Waitable to a wait set.
   /**
    * \param[in] wait_set A handle to the wait set to add the Waitable to.
@@ -126,24 +109,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  add_to_wait_set(rcl_wait_set_t & wait_set);
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Woverloaded-virtual"
-#endif
-  /// Deprecated.
-  /**
-   * \sa is_ready(const rcl_wait_set_t &)
-   */
-  [[deprecated("Use is_ready(const rcl_wait_set_t & wait_set) signature")]]
-  RCLCPP_PUBLIC
-  virtual
-  bool
-  is_ready(rcl_wait_set_t * wait_set);
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
+  add_to_wait_set(rcl_wait_set_t & wait_set) = 0;
 
   /// Check if the Waitable is ready.
   /**
@@ -158,7 +124,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   bool
-  is_ready(const rcl_wait_set_t & wait_set);
+  is_ready(const rcl_wait_set_t & wait_set) = 0;
 
   /// Take the data so that it can be consumed with `execute`.
   /**
@@ -210,24 +176,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   std::shared_ptr<void>
-  take_data_by_entity_id(size_t id);
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Woverloaded-virtual"
-#endif
-  /// Deprecated.
-  /**
-   * \sa execute(const std::shared_ptr<void> &)
-   */
-  [[deprecated("Use execute(const std::shared_ptr<void> & data) signature")]]
-  RCLCPP_PUBLIC
-  virtual
-  void
-  execute(std::shared_ptr<void> & data);
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
+  take_data_by_entity_id(size_t id) = 0;
 
   /// Execute data that is passed in.
   /**
@@ -254,7 +203,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  execute(const std::shared_ptr<void> & data);
+  execute(const std::shared_ptr<void> & data) = 0;
 
   /// Exchange the "in use by wait set" state for this timer.
   /**
@@ -297,7 +246,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  set_on_ready_callback(std::function<void(size_t, int)> callback);
+  set_on_ready_callback(std::function<void(size_t, int)> callback) = 0;
 
   /// Unset any callback registered via set_on_ready_callback.
   /**
@@ -307,7 +256,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  clear_on_ready_callback();
+  clear_on_ready_callback() = 0;
 
 private:
   std::atomic<bool> in_use_by_wait_set_{false};

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>28.1.0</version>
+  <version>29.3.0</version>
   <description>The ROS client library in C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp/src/rclcpp/context.cpp

@@ -212,28 +212,27 @@ Context::init(
   }
   rcl_context_.reset(context, __delete_context);
 
-  if (init_options.auto_initialize_logging()) {
-    logging_mutex_ = get_global_logging_mutex();
-    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
-    size_t & count = get_logging_reference_count();
-    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()),
-        rclcpp_logging_output_handler);
-      if (RCL_RET_OK != ret) {
-        rcl_context_.reset();
-        rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
-      }
-    } else {
-      RCLCPP_WARN(
-        rclcpp::get_logger("rclcpp"),
-        "logging was initialized more than once");
-    }
-    ++count;
-  }
-
   try {
+    if (init_options.auto_initialize_logging()) {
+      logging_mutex_ = get_global_logging_mutex();
+      std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+      size_t & count = get_logging_reference_count();
+      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()),
+          rclcpp_logging_output_handler);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
+        }
+      } else {
+        RCLCPP_WARN(
+          rclcpp::get_logger("rclcpp"),
+          "logging was initialized more than once");
+      }
+      ++count;
+    }
+
     std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
       argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
     if (!unparsed_ros_arguments.empty()) {

rclcpp/src/rclcpp/create_generic_service.cpp

@@ -0,0 +1,49 @@
+// Copyright 2024 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 <string>
+#include <memory>
+
+#include "rclcpp/create_generic_service.hpp"
+#include "rclcpp/generic_service.hpp"
+
+namespace rclcpp
+{
+rclcpp::GenericService::SharedPtr
+create_generic_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  GenericServiceCallback any_callback,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rcl_service_options_t options = rcl_service_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+
+  auto srv = rclcpp::GenericService::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    service_type,
+    any_callback,
+    options);
+
+  auto srv_base_ptr = std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv);
+  node_services->add_service(srv_base_ptr, group);
+  return srv;
+}
+}  // namespace rclcpp

rclcpp/src/rclcpp/dynamic_subscription.cpp

@@ -0,0 +1,113 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/dynamic_subscription.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rcl/subscription.h"
+
+#include "rclcpp/exceptions.hpp"
+
+namespace rclcpp
+{
+
+std::shared_ptr<void> DynamicSubscription::create_message()
+{
+  return create_serialized_message();
+}
+
+std::shared_ptr<rclcpp::SerializedMessage> DynamicSubscription::create_serialized_message()
+{
+  return std::make_shared<rclcpp::SerializedMessage>(0);
+}
+
+void DynamicSubscription::handle_message(std::shared_ptr<void> &, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::handle_serialized_message(
+  const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_serialized_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::handle_loaned_message(void *, const rclcpp::MessageInfo &)
+{
+  throw rclcpp::exceptions::UnimplementedError(
+          "handle_loaned_message is not implemented for DynamicSubscription");
+}
+
+void DynamicSubscription::return_message(std::shared_ptr<void> & message)
+{
+  auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
+  return_serialized_message(typed_message);
+}
+
+void DynamicSubscription::return_serialized_message(
+  std::shared_ptr<rclcpp::SerializedMessage> & message)
+{
+  message.reset();
+}
+
+
+// DYNAMIC TYPE ====================================================================================
+// TODO(methylDragon): Re-order later
+
+// Does not clone
+rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
+DynamicSubscription::get_shared_dynamic_message_type()
+{
+  return dynamic_message_type_;
+}
+
+// Does not clone
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+DynamicSubscription::get_shared_dynamic_message()
+{
+  return dynamic_message_;
+}
+
+rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
+DynamicSubscription::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
+DynamicSubscription::create_dynamic_message()
+{
+  return dynamic_message_->init_from_type_shared(*dynamic_message_type_);
+}
+
+void
+DynamicSubscription::return_dynamic_message(
+  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
+{
+  message.reset();
+}
+
+void DynamicSubscription::handle_dynamic_message(
+  const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
+  const rclcpp::MessageInfo & message_info)
+{
+  (void) message_info;
+  callback_(message, ts_->get_rosidl_runtime_c_type_description());
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp

@@ -36,5 +36,734 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
 using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
 using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp"
+#endif
+
+
+// CONSTRUCTION ==================================================================================
+DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
+: DynamicMessage::DynamicMessage(
+    dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {}
+
+
+DynamicMessage::DynamicMessage(
+  const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder,
+  rcl_allocator_t allocator)
+: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!serialization_support_) {
+    throw std::runtime_error("dynamic message could not bind serialization support!");
+  }
+  if (!dynamic_type_builder) {
+    throw std::runtime_error("dynamic message type builder cannot be nullptr!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder =
+    dynamic_type_builder->get_rosidl_dynamic_type_builder();
+
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type_builder(
+    &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_data());
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic data object from dynamic type builder");
+  }
+}
+
+
+DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type)
+: DynamicMessage::DynamicMessage(
+    dynamic_type, dynamic_type->get_rosidl_dynamic_type().allocator) {}
+
+
+DynamicMessage::DynamicMessage(
+  const DynamicMessageType::SharedPtr dynamic_type,
+  rcl_allocator_t allocator)
+: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data()),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (!serialization_support_) {
+    throw std::runtime_error("dynamic type could not bind serialization support!");
+  }
+  if (!dynamic_type) {
+    throw std::runtime_error("dynamic message type cannot be nullptr!");
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type =
+    dynamic_type->get_rosidl_dynamic_type();
+
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type(
+    &rosidl_dynamic_type, &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not init new dynamic data object from dynamic type") +
+            rcl_get_error_string().str);
+  }
+}
+
+
+DynamicMessage::DynamicMessage(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_data_t && rosidl_dynamic_data)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_data_(std::move(rosidl_dynamic_data)),
+  is_loaned_(false),
+  parent_data_(nullptr)
+{
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, rosidl_dynamic_data)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic data's!");
+    }
+  }
+}
+
+
+DynamicMessage::DynamicMessage(
+  DynamicMessage::SharedPtr parent_data,
+  rosidl_dynamic_typesupport_dynamic_data_t && rosidl_loaned_data)
+: serialization_support_(parent_data->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_data_(std::move(rosidl_loaned_data)),
+  is_loaned_(true),
+  parent_data_(nullptr)
+{
+  if (!parent_data) {
+    throw std::runtime_error("parent dynamic data cannot be nullptr!");
+  }
+  if (serialization_support_) {
+    if (!match_serialization_support_(*serialization_support_, rosidl_loaned_data)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match loaned dynamic data's!");
+    }
+  }
+  parent_data_ = parent_data;
+}
+
+
+DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_data_(std::exchange(
+      other.rosidl_dynamic_data_,
+      rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data())),
+  is_loaned_(other.is_loaned_),
+  parent_data_(std::exchange(other.parent_data_, nullptr))
+{}
+
+
+DynamicMessage &
+DynamicMessage::operator=(DynamicMessage && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_);
+  is_loaned_ = other.is_loaned_;
+  std::swap(parent_data_, other.parent_data_);
+  return *this;
+}
+
+
 DynamicMessage::~DynamicMessage()
-{}  // STUBBED
+{
+  if (!is_loaned_) {
+    if (rosidl_dynamic_typesupport_dynamic_data_fini(&get_rosidl_dynamic_data()) !=
+      RCUTILS_RET_OK)
+    {
+      RCUTILS_LOG_ERROR("could not fini rosidl dynamic data");
+    }
+    return;
+  }
+
+  // Loaned case
+  if (!parent_data_) {
+    RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!");
+  } else {
+    rosidl_dynamic_typesupport_dynamic_data_return_loaned_value(
+      &parent_data_->get_rosidl_dynamic_data(), &get_rosidl_dynamic_data());
+  }
+}
+
+
+bool
+DynamicMessage::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data)
+{
+  if (serialization_support.get_serialization_library_identifier() != std::string(
+      rosidl_dynamic_type_data.serialization_support->serialization_library_identifier))
+  {
+    RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's");
+    return false;
+  }
+  return true;
+}
+
+
+// GETTERS =======================================================================================
+const std::string
+DynamicMessage::get_serialization_library_identifier() const
+{
+  return std::string(
+    get_rosidl_dynamic_data().serialization_support->serialization_library_identifier);
+}
+
+
+const std::string
+DynamicMessage::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  if (
+    rosidl_dynamic_typesupport_dynamic_data_get_name(
+      &get_rosidl_dynamic_data(), &buf,
+      &buf_length) !=
+    RCUTILS_RET_OK)
+  {
+    throw std::runtime_error(
+            std::string("could not get name for dynamic data") + rcl_get_error_string().str);
+  }
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_data_t &
+DynamicMessage::get_rosidl_dynamic_data()
+{
+  return rosidl_dynamic_data_;
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_data_t &
+DynamicMessage::get_rosidl_dynamic_data() const
+{
+  return rosidl_dynamic_data_;
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessage::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessage::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+size_t
+DynamicMessage::get_item_count() const
+{
+  size_t item_count;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count(
+    &get_rosidl_dynamic_data(), &item_count);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not get item count of dynamic data");
+  }
+  return item_count;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_member_id(size_t index) const
+{
+  rosidl_dynamic_typesupport_member_id_t member_id;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index(
+    &get_rosidl_dynamic_data(), index, &member_id);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not member id of dynamic data element by index");
+  }
+  return member_id;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_member_id(const std::string & name) const
+{
+  rosidl_dynamic_typesupport_member_id_t member_id;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name(
+    &get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not member id of dynamic data element by name");
+  }
+  return member_id;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_array_index(size_t index) const
+{
+  rosidl_dynamic_typesupport_member_id_t array_index;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index(
+    &get_rosidl_dynamic_data(), index, &array_index);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not array index of dynamic data element by index");
+  }
+  return array_index;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::get_array_index(const std::string & name) const
+{
+  return get_array_index(get_member_id(name));
+}
+
+
+// METHODS =======================================================================================
+DynamicMessage
+DynamicMessage::clone(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
+    &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data));
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::clone_shared(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
+    &get_rosidl_dynamic_data(), &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_data));
+}
+
+
+DynamicMessage
+DynamicMessage::init_from_type(DynamicMessageType & type, rcl_allocator_t allocator) const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type(
+    &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic data object from dynamic type");
+  }
+  return DynamicMessage(serialization_support_, std::move(rosidl_dynamic_data));
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::init_from_type_shared(DynamicMessageType & type, rcl_allocator_t allocator) const
+{
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_init_from_dynamic_type(
+    &type.get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic data object from dynamic type");
+  }
+  return DynamicMessage::make_shared(serialization_support_, std::move(rosidl_dynamic_data));
+}
+
+
+bool
+DynamicMessage::equals(const DynamicMessage & other) const
+{
+  if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  bool equals;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals(
+    &get_rosidl_dynamic_data(), &other.get_rosidl_dynamic_data(), &equals);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not equate dynamic messages");
+  }
+  return equals;
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::loan_value(
+  rosidl_dynamic_typesupport_member_id_t id,
+  rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value(
+    &get_rosidl_dynamic_data(), id, &allocator, &rosidl_dynamic_data);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessage::make_shared(shared_from_this(), std::move(rosidl_dynamic_data));
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::loan_value(const std::string & name, rcl_allocator_t allocator)
+{
+  return loan_value(get_member_id(name), allocator);
+}
+
+
+void
+DynamicMessage::clear_all_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_all_values(&get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::clear_nonkey_values()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(&get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id)
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_value(&get_rosidl_dynamic_data(), id);
+}
+
+
+void
+DynamicMessage::clear_value(const std::string & name)
+{
+  clear_value(get_member_id(name));
+}
+
+
+void
+DynamicMessage::clear_sequence()
+{
+  rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(&get_rosidl_dynamic_data());
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_sequence_data()
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(&get_rosidl_dynamic_data(), &out);
+  return out;
+}
+
+
+void
+DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index)
+{
+  rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data(
+    &get_rosidl_dynamic_data(), index);
+}
+
+
+bool
+DynamicMessage::serialize(rcl_serialized_message_t & buffer)
+{
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_serialize(
+    &get_rosidl_dynamic_data(), &buffer);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return true;
+}
+
+
+bool
+DynamicMessage::deserialize(rcl_serialized_message_t & buffer)
+{
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_deserialize(
+    &get_rosidl_dynamic_data(), &buffer);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str);
+  }
+  return true;
+}
+
+
+// MEMBER ACCESS ===================================================================================
+// Defined in "detail/dynamic_message_impl.hpp"
+
+
+// FIXED STRING MEMBER ACCESS ======================================================================
+const std::string
+DynamicMessage::get_fixed_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t string_length)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::string
+DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length)
+{
+  return get_fixed_string_value(get_member_id(name), string_length);
+}
+
+
+const std::u16string
+DynamicMessage::get_fixed_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::u16string
+DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length)
+{
+  return get_fixed_wstring_value(get_member_id(name), wstring_length);
+}
+
+
+void
+DynamicMessage::set_fixed_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value(
+    &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length);
+}
+
+
+void
+DynamicMessage::set_fixed_string_value(
+  const std::string & name, const std::string value, size_t string_length)
+{
+  set_fixed_string_value(get_member_id(name), value, string_length);
+}
+
+
+void
+DynamicMessage::set_fixed_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value(
+    &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length);
+}
+
+
+void
+DynamicMessage::set_fixed_wstring_value(
+  const std::string & name, const std::u16string value, size_t wstring_length)
+{
+  set_fixed_wstring_value(get_member_id(name), value, wstring_length);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value(
+    &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value(
+    &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out);
+  return out;
+}
+
+
+// BOUNDED STRING MEMBER ACCESS ====================================================================
+const std::string
+DynamicMessage::get_bounded_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t string_bound)
+{
+  size_t buf_length;
+  char * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound);
+  auto out = std::string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::string
+DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound)
+{
+  return get_bounded_string_value(get_member_id(name), string_bound);
+}
+
+
+const std::u16string
+DynamicMessage::get_bounded_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound)
+{
+  size_t buf_length;
+  char16_t * buf = nullptr;
+  rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value(
+    &get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound);
+  auto out = std::u16string(buf, buf_length);
+  delete buf;
+  return out;
+}
+
+
+const std::u16string
+DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound)
+{
+  return get_bounded_wstring_value(get_member_id(name), wstring_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_string_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value(
+    &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_string_value(
+  const std::string & name, const std::string value, size_t string_bound)
+{
+  set_bounded_string_value(get_member_id(name), value, string_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_wstring_value(
+  rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value(
+    &get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound);
+}
+
+
+void
+DynamicMessage::set_bounded_wstring_value(
+  const std::string & name, const std::u16string value, size_t wstring_bound)
+{
+  set_bounded_wstring_value(get_member_id(name), value, wstring_bound);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value(
+    &get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value(
+    &get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out);
+  return out;
+}
+
+
+// NESTED MEMBER ACCESS ============================================================================
+DynamicMessage
+DynamicMessage::get_nested_data(
+  rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t out =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rosidl_dynamic_typesupport_dynamic_data_get_nested_data(
+    &get_rosidl_dynamic_data(), id, &allocator, &out);
+  return DynamicMessage(get_shared_dynamic_serialization_support(), std::move(out));
+}
+
+
+DynamicMessage
+DynamicMessage::get_nested_data(const std::string & name, rcl_allocator_t allocator)
+{
+  return get_nested_data(get_member_id(name), allocator);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::get_nested_data_shared(
+  rosidl_dynamic_typesupport_member_id_t id, rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_data_t out =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_data();
+  rosidl_dynamic_typesupport_dynamic_data_get_nested_data(
+    &get_rosidl_dynamic_data(), id, &allocator, &out);
+  return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), std::move(out));
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessage::get_nested_data_shared(const std::string & name, rcl_allocator_t allocator)
+{
+  return get_nested_data_shared(get_member_id(name), allocator);
+}
+
+
+void
+DynamicMessage::set_nested_data(
+  rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_dynamic_data_set_nested_data(
+    &get_rosidl_dynamic_data(), id, &value.get_rosidl_dynamic_data());
+}
+
+
+void
+DynamicMessage::set_nested_data(const std::string & name, DynamicMessage & value)
+{
+  set_nested_data(get_member_id(name), value);
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_nested_data_copy(const DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_nested_data_copy(
+    &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out);
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_member_id_t
+DynamicMessage::insert_nested_data(DynamicMessage & value)
+{
+  rosidl_dynamic_typesupport_member_id_t out;
+  rosidl_dynamic_typesupport_dynamic_data_insert_nested_data(
+    &get_rosidl_dynamic_data(), &value.get_rosidl_dynamic_data(), &out);
+  return out;
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp

@@ -34,5 +34,248 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
 using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
 using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
+: DynamicMessageType::DynamicMessageType(
+    dynamic_type_builder, dynamic_type_builder->get_rosidl_dynamic_type_builder().allocator) {}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder,
+  rcl_allocator_t allocator)
+: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
+  rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type())
+{
+  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();
+
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_dynamic_type_builder(
+    &rosidl_dynamic_type_builder, &allocator, &get_rosidl_dynamic_type());
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic type object");
+  }
+}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_type_t && rosidl_dynamic_type)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_(std::move(rosidl_dynamic_type))
+{
+  if (serialization_support) {
+    if (!match_serialization_support_(*serialization_support, rosidl_dynamic_type)) {
+      throw std::runtime_error(
+              "serialization support library identifier does not match dynamic type's!");
+    }
+  }
+}
+
+
+DynamicMessageType::DynamicMessageType(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_(rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type())
+{
+  init_from_description(description, allocator, serialization_support);
+}
+
+
+DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_(std::exchange(
+      other.rosidl_dynamic_type_, rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type()))
+{}
+
+
+DynamicMessageType &
+DynamicMessageType::operator=(DynamicMessageType && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_);
+  return *this;
+}
+
+
 DynamicMessageType::~DynamicMessageType()
-{}  // STUBBED
+{
+  if (rosidl_dynamic_typesupport_dynamic_type_fini(&get_rosidl_dynamic_type()) != RCUTILS_RET_OK) {
+    RCUTILS_LOG_ERROR("could not fini rosidl dynamic type");
+  }
+}
+
+
+void
+DynamicMessageType::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (serialization_support) {
+    // Swap serialization support if serialization support is given
+    serialization_support_ = serialization_support;
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_init_from_description(
+    &serialization_support_->get_rosidl_serialization_support(),
+    &description,
+    &allocator,
+    &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic type object");
+  }
+
+  rosidl_dynamic_type_ = std::move(rosidl_dynamic_type);
+}
+
+
+bool
+DynamicMessageType::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type)
+{
+  if (serialization_support.get_serialization_library_identifier() != std::string(
+      rosidl_dynamic_type.serialization_support->serialization_library_identifier))
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support library identifier does not match dynamic type's (%s vs %s)",
+      serialization_support.get_serialization_library_identifier().c_str(),
+      rosidl_dynamic_type.serialization_support->serialization_library_identifier);
+    return false;
+  }
+  return true;
+}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicMessageType::get_serialization_library_identifier() const
+{
+  return std::string(
+    get_rosidl_dynamic_type().serialization_support->serialization_library_identifier);
+}
+
+
+const std::string
+DynamicMessageType::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  rosidl_dynamic_typesupport_dynamic_type_get_name(&get_rosidl_dynamic_type(), &buf, &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+size_t
+DynamicMessageType::get_member_count() const
+{
+  size_t out;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count(
+    &get_rosidl_dynamic_type(), &out);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not get member count: ") + rcl_get_error_string().str);
+  }
+  return out;
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_t &
+DynamicMessageType::get_rosidl_dynamic_type()
+{
+  return rosidl_dynamic_type_;
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_t &
+DynamicMessageType::get_rosidl_dynamic_type() const
+{
+  return rosidl_dynamic_type_;
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageType::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageType::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =========================================================================================
+DynamicMessageType
+DynamicMessageType::clone(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
+    &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageType(
+    get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type));
+}
+
+
+DynamicMessageType::SharedPtr
+DynamicMessageType::clone_shared(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
+    &get_rosidl_dynamic_type(), &allocator, &rosidl_dynamic_type);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageType::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type));
+}
+
+
+bool
+DynamicMessageType::equals(const DynamicMessageType & other) const
+{
+  if (get_serialization_library_identifier() != other.get_serialization_library_identifier()) {
+    throw std::runtime_error("library identifiers don't match");
+  }
+  bool out;
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals(
+    &get_rosidl_dynamic_type(), &other.get_rosidl_dynamic_type(), &out);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not equate dynamic message types: ") + rcl_get_error_string().str);
+  }
+  return out;
+}
+
+
+DynamicMessage
+DynamicMessageType::build_dynamic_message(rcl_allocator_t allocator)
+{
+  return DynamicMessage(shared_from_this(), allocator);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessageType::build_dynamic_message_shared(rcl_allocator_t allocator)
+{
+  return DynamicMessage::make_shared(shared_from_this(), allocator);
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp

@@ -33,5 +33,582 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
 using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
 using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 
+#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
+// Template specialization implementations
+#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp"
+#endif
+
+// CONSTRUCTION ====================================================================================
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name)
+: DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+    serialization_support,
+    name,
+    serialization_support->get_rosidl_serialization_support().allocator) {}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const std::string & name,
+  rcl_allocator_t allocator)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_builder_(
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())
+{
+  init_from_serialization_support_(serialization_support, name, allocator);
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  rosidl_dynamic_typesupport_dynamic_type_builder_t && rosidl_dynamic_type_builder)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_builder_(
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support 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!");
+  }
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator)
+: serialization_support_(serialization_support),
+  rosidl_dynamic_type_builder_(
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())
+{
+  if (!serialization_support) {
+    throw std::runtime_error("serialization support cannot be nullptr!");
+  }
+  init_from_description(description, allocator, serialization_support);
+}
+
+
+DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept
+: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
+  rosidl_dynamic_type_builder_(std::exchange(
+      other.rosidl_dynamic_type_builder_,
+      rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder())) {}
+
+
+DynamicMessageTypeBuilder &
+DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept
+{
+  std::swap(serialization_support_, other.serialization_support_);
+  std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_);
+  return *this;
+}
+
+
 DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder()
-{}  // STUBBED
+{
+  if (rosidl_dynamic_typesupport_dynamic_type_builder_fini(&get_rosidl_dynamic_type_builder()) !=
+    RCUTILS_RET_OK)
+  {
+    RCUTILS_LOG_ERROR("could not fini rosidl dynamic type builder");
+  }
+}
+
+
+void
+DynamicMessageTypeBuilder::init_from_description(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator,
+  DynamicSerializationSupport::SharedPtr serialization_support)
+{
+  if (serialization_support) {
+    // Swap serialization support if serialization support is given
+    serialization_support_ = serialization_support;
+  }
+
+  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init_from_description(
+    &serialization_support_->get_rosidl_serialization_support(),
+    &description,
+    &allocator,
+    &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init new dynamic type builder object");
+  }
+
+  rosidl_dynamic_type_builder_ = std::move(rosidl_dynamic_type_builder);
+}
+
+
+void
+DynamicMessageTypeBuilder::init_from_serialization_support_(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const std::string & name,
+  rcl_allocator_t allocator)
+{
+  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 =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_init(
+    &serialization_support->get_rosidl_serialization_support(),
+    name.c_str(), name.size(),
+    &allocator,
+    &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not init dynamic type builder: ") + rcl_get_error_string().str);
+  }
+}
+
+
+bool
+DynamicMessageTypeBuilder::match_serialization_support_(
+  const DynamicSerializationSupport & serialization_support,
+  const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder)
+{
+  if (serialization_support.get_serialization_library_identifier() != std::string(
+      rosidl_dynamic_type_builder.serialization_support->serialization_library_identifier))
+  {
+    RCUTILS_LOG_ERROR(
+      "serialization support library identifier does not match dynamic type builder's");
+    return false;
+  }
+  return true;
+}
+
+
+// GETTERS =======================================================================================
+const std::string
+DynamicMessageTypeBuilder::get_serialization_library_identifier() const
+{
+  return std::string(
+    get_rosidl_dynamic_type_builder().serialization_support->serialization_library_identifier);
+}
+
+
+const std::string
+DynamicMessageTypeBuilder::get_name() const
+{
+  size_t buf_length;
+  const char * buf;
+  rosidl_dynamic_typesupport_dynamic_type_builder_get_name(
+    &get_rosidl_dynamic_type_builder(), &buf, &buf_length);
+  return std::string(buf, buf_length);
+}
+
+
+rosidl_dynamic_typesupport_dynamic_type_builder_t &
+DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder()
+{
+  return rosidl_dynamic_type_builder_;
+}
+
+
+const rosidl_dynamic_typesupport_dynamic_type_builder_t &
+DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const
+{
+  return rosidl_dynamic_type_builder_;
+}
+
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+
+// METHODS =======================================================================================
+void
+DynamicMessageTypeBuilder::set_name(const std::string & name)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_set_name(
+    &get_rosidl_dynamic_type_builder(), name.c_str(), name.size());
+}
+
+
+DynamicMessageTypeBuilder
+DynamicMessageTypeBuilder::clone(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
+    &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageTypeBuilder(
+    get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder));
+}
+
+
+DynamicMessageTypeBuilder::SharedPtr
+DynamicMessageTypeBuilder::clone_shared(rcl_allocator_t allocator)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder =
+    rosidl_dynamic_typesupport_get_zero_initialized_dynamic_type_builder();
+  rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
+    &get_rosidl_dynamic_type_builder(), &allocator, &rosidl_dynamic_type_builder);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error(
+            std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
+  }
+  return DynamicMessageTypeBuilder::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(rosidl_dynamic_type_builder));
+}
+
+
+void
+DynamicMessageTypeBuilder::clear(rcl_allocator_t allocator)
+{
+  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, allocator);
+}
+
+
+DynamicMessage
+DynamicMessageTypeBuilder::build_dynamic_message(rcl_allocator_t allocator)
+{
+  return DynamicMessage(shared_from_this(), allocator);
+}
+
+
+DynamicMessage::SharedPtr
+DynamicMessageTypeBuilder::build_dynamic_message_shared(rcl_allocator_t allocator)
+{
+  return DynamicMessage::make_shared(shared_from_this(), allocator);
+}
+
+
+DynamicMessageType
+DynamicMessageTypeBuilder::build_dynamic_message_type(rcl_allocator_t allocator)
+{
+  return DynamicMessageType(shared_from_this(), allocator);
+}
+
+
+DynamicMessageType::SharedPtr
+DynamicMessageTypeBuilder::build_dynamic_message_type_shared(rcl_allocator_t allocator)
+{
+  return DynamicMessageType::make_shared(shared_from_this(), allocator);
+}
+
+
+// ADD MEMBERS =====================================================================================
+// Defined in "detail/dynamic_message_type_builder_impl.hpp"
+
+
+// ADD FIXED STRING MEMBERS ========================================================================
+void
+DynamicMessageTypeBuilder::add_fixed_string_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_length, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_length, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_length, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_length, sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_length, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_length, sequence_bound);
+}
+
+
+// ADD BOUNDED STRING MEMBERS ======================================================================
+void
+DynamicMessageTypeBuilder::add_bounded_string_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_bound, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_bound, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound, array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t string_bound, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    string_bound, sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  size_t wstring_bound, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    wstring_bound, sequence_bound);
+}
+
+
+// ADD NESTED MEMBERS ==============================================================================
+void
+DynamicMessageTypeBuilder::add_complex_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_array_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, size_t array_length, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type.get_rosidl_dynamic_type(), array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type.get_rosidl_dynamic_type());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_bounded_sequence_member(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type.get_rosidl_dynamic_type(), sequence_bound);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_array_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, size_t array_length,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type_builder.get_rosidl_dynamic_type_builder(), array_length);
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type_builder.get_rosidl_dynamic_type_builder());
+}
+
+
+void
+DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder(
+  rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
+  DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound,
+  const std::string & default_value)
+{
+  rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder(
+    &get_rosidl_dynamic_type_builder(),
+    id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
+    &nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound);
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp

@@ -45,5 +45,273 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
 using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
 using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 
+// CONSTRUCTION ====================================================================================
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  const std::string & serialization_library_name,
+  rcl_allocator_t allocator)
+: serialization_support_(nullptr),
+  dynamic_message_type_(nullptr),
+  dynamic_message_(nullptr),
+  rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle())
+{
+  rcl_ret_t ret;
+  if (serialization_library_name.empty()) {
+    ret = rcl_dynamic_message_type_support_handle_init(
+      nullptr, &description, &allocator, &rosidl_message_type_support_);
+  } else {
+    ret = rcl_dynamic_message_type_support_handle_init(
+      serialization_library_name.c_str(), &description, &allocator, &rosidl_message_type_support_);
+  }
+  if (ret != RCL_RET_OK) {
+    std::string error_msg =
+      std::string("error initializing rosidl message type support: ") + rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(error_msg);
+  }
+  if (rosidl_message_type_support_.typesupport_identifier !=
+    rosidl_get_dynamic_typesupport_identifier())
+  {
+    throw std::runtime_error("rosidl message type support is of the wrong type");
+  }
+
+  auto ts_impl = static_cast<rosidl_dynamic_message_type_support_impl_t *>(const_cast<void *>(
+      rosidl_message_type_support_.data));
+
+  serialization_support_ = DynamicSerializationSupport::make_shared(
+    std::move(ts_impl->serialization_support));
+
+  dynamic_message_type_ = DynamicMessageType::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type));
+
+  dynamic_message_ = DynamicMessage::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message));
+}
+
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator)
+: serialization_support_(serialization_support),
+  dynamic_message_type_(nullptr),
+  dynamic_message_(nullptr),
+  rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle())
+{
+  // Check null
+  if (!serialization_support) {
+    throw std::runtime_error("serialization_support cannot be nullptr.");
+  }
+
+  rosidl_type_hash_t type_hash;
+  rcutils_ret_t hash_ret = rcl_calculate_type_hash(
+    // TODO(methylDragon): Swap this out with the conversion function when it is ready
+    reinterpret_cast<const type_description_interfaces__msg__TypeDescription *>(&description),
+    &type_hash);
+  if (hash_ret != RCL_RET_OK) {
+    throw std::runtime_error("failed to get type hash");
+  }
+
+  rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_init(
+    &serialization_support->get_rosidl_serialization_support(),
+    &type_hash,       // type_hash
+    &description,     // type_description
+    nullptr,          // type_description_sources (not implemented for dynamic types)
+    &allocator,
+    &rosidl_message_type_support_);
+  if (ret != RCUTILS_RET_OK) {
+    throw std::runtime_error("could not init rosidl message type support");
+  }
+  if (rosidl_message_type_support_.typesupport_identifier !=
+    rosidl_get_dynamic_typesupport_identifier())
+  {
+    throw std::runtime_error("rosidl message type support is of the wrong type");
+  }
+
+  auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(
+    rosidl_message_type_support_.data);
+
+  dynamic_message_type_ = DynamicMessageType::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message_type));
+
+  dynamic_message_ = DynamicMessage::make_shared(
+    get_shared_dynamic_serialization_support(), std::move(*ts_impl->dynamic_message));
+}
+
+DynamicMessageTypeSupport::DynamicMessageTypeSupport(
+  DynamicSerializationSupport::SharedPtr serialization_support,
+  DynamicMessageType::SharedPtr dynamic_message_type,
+  DynamicMessage::SharedPtr dynamic_message,
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator)
+: serialization_support_(serialization_support),
+  dynamic_message_type_(dynamic_message_type),
+  dynamic_message_(dynamic_message),
+  rosidl_message_type_support_(rosidl_get_zero_initialized_message_type_support_handle())
+{
+  // 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.");
+  }
+
+  // Check identifiers
+  if (serialization_support->get_serialization_library_identifier() !=
+    dynamic_message_type->get_serialization_library_identifier())
+  {
+    throw std::runtime_error(
+            "serialization support library identifier does not match "
+            "dynamic message type library identifier.");
+  }
+  if (dynamic_message_type->get_serialization_library_identifier() !=
+    dynamic_message->get_serialization_library_identifier())
+  {
+    throw std::runtime_error(
+            "dynamic message type library identifier does not match "
+            "dynamic message library identifier.");
+  }
+
+  rosidl_type_hash_t type_hash;
+  rcutils_ret_t hash_ret = rcl_calculate_type_hash(
+    // TODO(methylDragon): Swap this out with the conversion function when it is ready
+    //   from https://github.com/ros2/rcl/pull/1052
+    reinterpret_cast<const type_description_interfaces__msg__TypeDescription *>(&description),
+    &type_hash);
+  if (hash_ret != RCL_RET_OK) {
+    std::string error_msg = std::string("failed to get type hash: ") + rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(error_msg);
+  }
+
+  auto ts_impl = static_cast<rosidl_dynamic_message_type_support_impl_t *>(
+    allocator.zero_allocate(1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator.state)
+  );
+  if (!ts_impl) {
+    throw std::runtime_error("could not allocate rosidl_message_type_support_t");
+  }
+
+  ts_impl->allocator = allocator;
+  ts_impl->type_hash = type_hash;
+  if (!rosidl_runtime_c__type_description__TypeDescription__copy(
+      &description, &ts_impl->type_description))
+  {
+    throw std::runtime_error("could not copy type description");
+  }
+  // ts_impl->type_description_sources  = // Not used
+
+  ts_impl->serialization_support = serialization_support->get_rosidl_serialization_support();
+  ts_impl->dynamic_message_type = &dynamic_message_type->get_rosidl_dynamic_type();
+  ts_impl->dynamic_message = &dynamic_message->get_rosidl_dynamic_data();
+
+  rosidl_message_type_support_ = {
+    rosidl_get_dynamic_typesupport_identifier(),            // typesupport_identifier
+    ts_impl,                                                // data
+    get_message_typesupport_handle_function,                // func
+    // get_type_hash_func
+    rosidl_get_dynamic_message_type_support_type_hash_function,
+    // get_type_description_func
+    rosidl_get_dynamic_message_type_support_type_description_function,
+    // get_type_description_sources_func
+    rosidl_get_dynamic_message_type_support_type_description_sources_function
+  };
+}
+
 DynamicMessageTypeSupport::~DynamicMessageTypeSupport()
-{}  // STUBBED
+{
+  // These must go first
+  serialization_support_.reset();
+  dynamic_message_type_.reset();
+  dynamic_message_.reset();
+
+  // Early return if type support isn't populated to avoid segfaults
+  if (!rosidl_message_type_support_.data) {
+    return;
+  }
+
+  // We only partially finalize the rosidl_message_type_support->data since its pointer members are
+  // managed by their respective SharedPtr wrapper classes.
+  auto ts_impl = static_cast<rosidl_dynamic_message_type_support_impl_t *>(
+    const_cast<void *>(rosidl_message_type_support_.data)
+  );
+  rcutils_allocator_t allocator = ts_impl->allocator;
+
+  rosidl_runtime_c__type_description__TypeDescription__fini(&ts_impl->type_description);
+  rosidl_runtime_c__type_description__TypeSource__Sequence__fini(
+    &ts_impl->type_description_sources);
+  allocator.deallocate(static_cast<void *>(ts_impl), allocator.state);  // Always C allocated
+}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicMessageTypeSupport::get_serialization_library_identifier() const
+{
+  return serialization_support_->get_serialization_library_identifier();
+}
+
+rosidl_message_type_support_t &
+DynamicMessageTypeSupport::get_rosidl_message_type_support()
+{
+  return rosidl_message_type_support_;
+}
+
+const rosidl_message_type_support_t &
+DynamicMessageTypeSupport::get_const_rosidl_message_type_support()
+{
+  return rosidl_message_type_support_;
+}
+
+const rosidl_message_type_support_t &
+DynamicMessageTypeSupport::get_const_rosidl_message_type_support() const
+{
+  return rosidl_message_type_support_;
+}
+
+const rosidl_runtime_c__type_description__TypeDescription &
+DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const
+{
+  auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(
+    get_const_rosidl_message_type_support().data
+  );
+  return ts_impl->type_description;
+}
+
+DynamicSerializationSupport::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_serialization_support()
+{
+  return serialization_support_;
+}
+
+DynamicSerializationSupport::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const
+{
+  return serialization_support_;
+}
+
+DynamicMessageType::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message_type()
+{
+  return dynamic_message_type_;
+}
+
+DynamicMessageType::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message_type() const
+{
+  return dynamic_message_type_;
+}
+
+DynamicMessage::SharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message()
+{
+  return dynamic_message_;
+}
+
+DynamicMessage::ConstSharedPtr
+DynamicMessageTypeSupport::get_shared_dynamic_message() const
+{
+  return dynamic_message_;
+}

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp

@@ -28,19 +28,71 @@ using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 
 // CONSTRUCTION ====================================================================================
 DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator)
-: DynamicSerializationSupport::DynamicSerializationSupport("", allocator)
-{
-  throw std::runtime_error("Unimplemented");
-}
+: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) {}
 
 DynamicSerializationSupport::DynamicSerializationSupport(
-  const std::string & /*serialization_library_name*/,
-  rcl_allocator_t /*allocator*/)
+  const std::string & serialization_library_name,
+  rcl_allocator_t allocator)
 : rosidl_serialization_support_(
     rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())
 {
-  throw std::runtime_error("Unimplemented");
+  rmw_ret_t ret = RMW_RET_ERROR;
+
+  if (serialization_library_name.empty()) {
+    ret = rmw_serialization_support_init(NULL, &allocator, &rosidl_serialization_support_);
+  } else {
+    ret = rmw_serialization_support_init(
+      serialization_library_name.c_str(), &allocator, &rosidl_serialization_support_);
+  }
+  if (ret != RCL_RET_OK) {
+    std::string error_msg =
+      std::string("could not initialize new serialization support object: ") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(error_msg);
+  }
+}
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  rosidl_dynamic_typesupport_serialization_support_t && rosidl_serialization_support)
+: rosidl_serialization_support_(std::move(rosidl_serialization_support)) {}
+
+DynamicSerializationSupport::DynamicSerializationSupport(
+  DynamicSerializationSupport && other) noexcept
+: rosidl_serialization_support_(std::exchange(
+      other.rosidl_serialization_support_,
+      rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())) {}
+
+DynamicSerializationSupport &
+DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept
+{
+  std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_);
+  return *this;
 }
 
 DynamicSerializationSupport::~DynamicSerializationSupport()
-{}  // STUBBED
+{
+  rosidl_dynamic_typesupport_serialization_support_fini(&rosidl_serialization_support_);
+}
+
+
+// GETTERS =========================================================================================
+const std::string
+DynamicSerializationSupport::get_serialization_library_identifier() const
+{
+  return std::string(
+    rosidl_dynamic_typesupport_serialization_support_get_library_identifier(
+      &rosidl_serialization_support_));
+}
+
+rosidl_dynamic_typesupport_serialization_support_t &
+DynamicSerializationSupport::get_rosidl_serialization_support()
+{
+  return rosidl_serialization_support_;
+}
+
+const rosidl_dynamic_typesupport_serialization_support_t &
+DynamicSerializationSupport::get_rosidl_serialization_support() const
+{
+  return rosidl_serialization_support_;
+}

rclcpp/src/rclcpp/executor.cpp

@@ -129,7 +129,7 @@ Executor::~Executor()
 }
 
 void
-Executor::trigger_entity_recollect(bool notify)
+Executor::handle_updated_entities(bool notify)
 {
   this->entities_need_rebuild_.store(true);
 
@@ -174,11 +174,11 @@ Executor::add_callback_group(
   this->collector_.add_callback_group(group_ptr);
 
   try {
-    this->trigger_entity_recollect(notify);
+    this->handle_updated_entities(notify);
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(
-              "Failed to trigger guard condition on callback group add: ") + ex.what());
+              "Failed to handle entities update on callback group add: ") + ex.what());
   }
 }
 
@@ -188,11 +188,11 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   this->collector_.add_node(node_ptr);
 
   try {
-    this->trigger_entity_recollect(notify);
+    this->handle_updated_entities(notify);
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(
-              "Failed to trigger guard condition on node add: ") + ex.what());
+              "Failed to handle entities update on node add: ") + ex.what());
   }
 }
 
@@ -204,11 +204,11 @@ Executor::remove_callback_group(
   this->collector_.remove_callback_group(group_ptr);
 
   try {
-    this->trigger_entity_recollect(notify);
+    this->handle_updated_entities(notify);
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(
-              "Failed to trigger guard condition on callback group remove: ") + ex.what());
+              "Failed to handle entities update on callback group remove: ") + ex.what());
   }
 }
 
@@ -224,11 +224,11 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
   this->collector_.remove_node(node_ptr);
 
   try {
-    this->trigger_entity_recollect(notify);
+    this->handle_updated_entities(notify);
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(
-              "Failed to trigger guard condition on node remove: ") + ex.what());
+              "Failed to handle entities update on node remove: ") + ex.what());
   }
 }
 
@@ -275,7 +275,7 @@ Executor::spin_until_future_complete_impl(
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_until_future_complete() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Do one item of work.
     spin_once_impl(timeout_left);
@@ -364,26 +364,54 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););
 
+  // clear the wait result and wait for work without blocking to collect the work
+  // for the first time
+  // both spin_some and spin_all wait for work at the beginning
+  wait_result_.reset();
+  wait_for_work(std::chrono::milliseconds(0));
+  bool just_waited = true;
+
+  // The logic of this while loop is as follows:
+  //
+  // - while not shutdown, and spinning (not canceled), and not max duration reached...
+  // - try to get an executable item to execute, and execute it if available
+  // - otherwise, reset the wait result, and ...
+  // - if there was no work available just after waiting, break the loop unconditionally
+  //   - this is appropriate for both spin_some and spin_all which use this function
+  // - else if exhaustive = true, then wait for work again
+  //   - this is only used for spin_all and not spin_some
+  // - else break
+  //   - this only occurs with spin_some
+  //
+  // The logic of this loop is subtle and should be carefully changed if at all.
+  // See also:
+  //   https://github.com/ros2/rclcpp/issues/2508
+  //   https://github.com/ros2/rclcpp/pull/2517
   while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
-    if (!wait_result_.has_value()) {
-      wait_for_work(std::chrono::milliseconds(0));
-    }
-
     AnyExecutable any_exec;
     if (get_next_ready_executable(any_exec)) {
       execute_any_executable(any_exec);
+      just_waited = false;
     } else {
-      // If nothing is ready, reset the result to signal we are
-      // ready to wait again
+      // if nothing is ready, reset the result to clear it
       wait_result_.reset();
-    }
 
-    if (!wait_result_.has_value() && !exhaustive) {
-      // In the case of spin some, then we can exit
-      // In the case of spin all, then we will allow ourselves to wait again.
-      break;
+      if (just_waited) {
+        // there was no work after just waiting, always exit in this case
+        // before the exhaustive condition can be checked
+        break;
+      }
+
+      if (exhaustive) {
+        // if exhaustive, wait for work again
+        // this only happens for spin_all; spin_some only waits at the start
+        wait_for_work(std::chrono::milliseconds(0));
+        just_waited = true;
+      } else {
+        break;
+      }
     }
   }
 }
@@ -403,7 +431,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_once() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););
   spin_once_impl(timeout);
 }
 
@@ -582,10 +610,25 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
       }
 
     // DYNAMIC SUBSCRIPTION ========================================================================
-    // Deliver dynamic message
+    // If a subscription is dynamic, then it will use its serialization-specific dynamic data.
+    //
+    // Two cases:
+    // - Dynamic type subscription using dynamic type stored in its own internal type support struct
+    // - Non-dynamic type subscription with no stored dynamic type
+    //   - Subscriptions of this type must be able to lookup the local message description to
+    //     generate a dynamic type at runtime!
+    //   - TODO(methylDragon): I won't be handling this case yet
     case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE:
       {
-        throw std::runtime_error("Unimplemented");
+        DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message();
+        take_and_do_error_handling(
+          "taking a dynamic message from topic",
+          subscription->get_topic_name(),
+          // This modifies the stored dynamic data in the DynamicMessage in-place
+          [&]() {return subscription->take_dynamic_message(*dynamic_message, message_info);},
+          [&]() {subscription->handle_dynamic_message(dynamic_message, message_info);});
+        subscription->return_dynamic_message(dynamic_message);
+        break;
       }
 
     default:
@@ -701,13 +744,33 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   // Clear any previous wait result
   this->wait_result_.reset();
 
+  // we need to make sure that callback groups don't get out of scope
+  // during the wait. As in jazzy, they are not covered by the DynamicStorage,
+  // we explicitly hold them here as a bugfix
+  std::vector<rclcpp::CallbackGroup::SharedPtr> cbgs;
+
   {
     std::lock_guard<std::mutex> guard(mutex_);
+
     if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
       this->collect_entities();
     }
+
+    auto callback_groups = this->collector_.get_all_callback_groups();
+    cbgs.resize(callback_groups.size());
+    for(const auto & w_ptr : callback_groups) {
+      auto shr_ptr = w_ptr.lock();
+      if(shr_ptr) {
+        cbgs.push_back(std::move(shr_ptr));
+      }
+    }
   }
+
   this->wait_result_.emplace(wait_set_.wait(timeout));
+
+  // drop references to the callback groups, before trying to execute anything
+  cbgs.clear();
+
   if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) {
     RCUTILS_LOG_WARN_NAMED(
       "rclcpp",

rclcpp/src/rclcpp/executor_options.cpp

@@ -0,0 +1,55 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/executor_options.hpp"
+
+using rclcpp::ExecutorOptions;
+
+namespace rclcpp
+{
+
+class ExecutorOptionsImplementation {};
+
+}  // namespace rclcpp
+
+ExecutorOptions::ExecutorOptions()
+: memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
+  context(rclcpp::contexts::get_global_default_context()),
+  max_conditions(0),
+  impl_(nullptr)
+{}
+
+ExecutorOptions::~ExecutorOptions()
+{}
+
+ExecutorOptions::ExecutorOptions(const ExecutorOptions & other)
+{
+  *this = other;
+}
+
+ExecutorOptions & ExecutorOptions::operator=(const ExecutorOptions & other)
+{
+  if (this == &other) {
+    return *this;
+  }
+
+  this->memory_strategy = other.memory_strategy;
+  this->context = other.context;
+  this->max_conditions = other.max_conditions;
+  if (nullptr != other.impl_) {
+    this->impl_ = std::make_unique<ExecutorOptionsImplementation>(*other.impl_);
+  }
+
+  return *this;
+}

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <iostream>
-
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executors/executor_notify_waitable.hpp"
 
@@ -91,9 +89,9 @@ ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set)
 }
 
 void
-ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & data)
+ExecutorNotifyWaitable::execute(const std::shared_ptr<void> & /*data*/)
 {
-  (void) data;
+  std::lock_guard<std::mutex> lock(execute_mutex_);
   this->execute_callback_();
 }
 
@@ -149,6 +147,14 @@ ExecutorNotifyWaitable::clear_on_ready_callback()
   }
 }
 
+RCLCPP_PUBLIC
+void
+ExecutorNotifyWaitable::set_execute_callback(std::function<void(void)> on_execute_callback)
+{
+  std::lock_guard<std::mutex> lock(execute_mutex_);
+  execute_callback_ = on_execute_callback;
+}
+
 void
 ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
 {

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -55,7 +55,7 @@ MultiThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -30,7 +30,7 @@ SingleThreadedExecutor::spin()
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin() called while already spinning");
   }
-  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+  RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false););
 
   // Clear any previous result and rebuild the waitset
   this->wait_result_.reset();

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -110,10 +110,29 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
 StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout)
 {
+  // we need to make sure that callback groups don't get out of scope
+  // during the wait. As in jazzy, they are not covered by the DynamicStorage,
+  // we explicitly hold them here as a bugfix
+  std::vector<rclcpp::CallbackGroup::SharedPtr> cbgs;
+
   if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
     this->collect_entities();
   }
+
+  auto callback_groups = this->collector_.get_all_callback_groups();
+  cbgs.resize(callback_groups.size());
+  for(const auto & w_ptr : callback_groups) {
+    auto shr_ptr = w_ptr.lock();
+    if(shr_ptr) {
+      cbgs.push_back(std::move(shr_ptr));
+    }
+  }
+
   auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
+
+  // drop references to the callback groups, before trying to execute anything
+  cbgs.clear();
+
   if (wait_result.kind() == WaitResultKind::Empty) {
     RCUTILS_LOG_WARN_NAMED(
       "rclcpp",

rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp

@@ -52,30 +52,36 @@ EventsExecutor::EventsExecutor(
   timers_manager_ =
     std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
 
-  this->current_entities_collection_ =
-    std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
+  entities_need_rebuild_ = false;
 
-  notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
+  this->setup_notify_waitable();
+
+  // Ensure that the entities collection is empty (the base class may have added elements
+  // that we are not interested in)
+  this->current_collection_.clear();
+
+  // Make sure that the notify waitable is immediately added to the collection
+  // to avoid missing events
+  this->add_notify_waitable_to_collection(current_collection_.waitables);
+}
+
+void
+EventsExecutor::setup_notify_waitable()
+{
+  // The base class already created this object but the events-executor
+  // needs different callbacks.
+  assert(notify_waitable_ && "The notify waitable should have already been constructed");
+
+  notify_waitable_->set_execute_callback(
     [this]() {
       // This callback is invoked when:
       // - the interrupt or shutdown guard condition is triggered:
       //    ---> we need to wake up the executor so that it can terminate
       // - a node or callback group guard condition is triggered:
       //    ---> the entities collection is changed, we need to update callbacks
-      notify_waitable_event_pushed_ = false;
-      this->refresh_current_collection_from_callback_groups();
+      this->handle_updated_entities(false);
     });
 
-  // Make sure that the notify waitable is immediately added to the collection
-  // to avoid missing events
-  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
-
-  notify_waitable_->add_guard_condition(interrupt_guard_condition_);
-  notify_waitable_->add_guard_condition(shutdown_guard_condition_);
-
-  notify_waitable_->set_on_ready_callback(
-    this->create_waitable_callback(notify_waitable_.get()));
-
   auto notify_waitable_entity_id = notify_waitable_.get();
   notify_waitable_->set_on_ready_callback(
     [this, notify_waitable_entity_id](size_t num_events, int waitable_data) {
@@ -85,7 +91,7 @@ EventsExecutor::EventsExecutor(
       // For the same reason, if an event of this type has already been pushed but it has not been
       // processed yet, we avoid pushing additional events.
       (void)num_events;
-      if (notify_waitable_event_pushed_.exchange(true)) {
+      if (entities_need_rebuild_.exchange(true)) {
         return;
       }
 
@@ -93,9 +99,6 @@ EventsExecutor::EventsExecutor(
       {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
       this->events_queue_->enqueue(event);
     });
-
-  this->entities_collector_ =
-    std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(notify_waitable_);
 }
 
 EventsExecutor::~EventsExecutor()
@@ -164,6 +167,14 @@ EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhau
       return false;
     };
 
+  // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check
+  // if entities need to be rebuilt here rather than letting the notify waitable event do it.
+  // A non-exhaustive spin would not check for work a second time, thus delaying the execution
+  // of some entities to the next invocation of spin.
+  if (!exhaustive) {
+    this->handle_updated_entities(false);
+  }
+
   // Get the number of events and timers ready at start
   const size_t ready_events_at_start = events_queue_->size();
   size_t executed_events = 0;
@@ -229,46 +240,6 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
   }
 }
 
-void
-EventsExecutor::add_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up the executor when a node is added.
-  (void) notify;
-
-  // Add node to entities collector
-  this->entities_collector_->add_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->add_node(node_ptr->get_node_base_interface(), notify);
-}
-
-void
-EventsExecutor::remove_node(
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up the executor when a node is removed.
-  (void)notify;
-
-  // Remove node from entities collector.
-  // This will result in un-setting all the event callbacks from its entities.
-  // After this function returns, this executor will not receive any more events associated
-  // to these entities.
-  this->entities_collector_->remove_node(node_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
-{
-  this->remove_node(node_ptr->get_node_base_interface(), notify);
-}
 
 void
 EventsExecutor::execute_event(const ExecutorEvent & event)
@@ -278,10 +249,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         rclcpp::ClientBase::SharedPtr client;
         {
-          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
           client = this->retrieve_entity(
             static_cast<const rcl_client_t *>(event.entity_key),
-            current_entities_collection_->clients);
+            current_collection_.clients);
         }
         if (client) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -295,10 +265,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         rclcpp::SubscriptionBase::SharedPtr subscription;
         {
-          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
           subscription = this->retrieve_entity(
             static_cast<const rcl_subscription_t *>(event.entity_key),
-            current_entities_collection_->subscriptions);
+            current_collection_.subscriptions);
         }
         if (subscription) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -311,10 +280,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         rclcpp::ServiceBase::SharedPtr service;
         {
-          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
           service = this->retrieve_entity(
             static_cast<const rcl_service_t *>(event.entity_key),
-            current_entities_collection_->services);
+            current_collection_.services);
         }
         if (service) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -334,10 +302,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
       {
         rclcpp::Waitable::SharedPtr waitable;
         {
-          std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
           waitable = this->retrieve_entity(
             static_cast<const rclcpp::Waitable *>(event.entity_key),
-            current_entities_collection_->waitables);
+            current_collection_.waitables);
         }
         if (waitable) {
           for (size_t i = 0; i < event.num_events; i++) {
@@ -351,61 +318,23 @@ EventsExecutor::execute_event(const ExecutorEvent & event)
 }
 
 void
-EventsExecutor::add_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr,
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  bool notify)
+EventsExecutor::handle_updated_entities(bool notify)
 {
-  // This field is unused because we don't have to wake up
-  // the executor when a callback group is added.
-  (void)notify;
-  (void)node_ptr;
-
-  this->entities_collector_->add_callback_group(group_ptr);
-
-  this->refresh_current_collection_from_callback_groups();
-}
-
-void
-EventsExecutor::remove_callback_group(
-  rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
-{
-  // This field is unused because we don't have to wake up
-  // the executor when a callback group is removed.
   (void)notify;
 
-  this->entities_collector_->remove_callback_group(group_ptr);
+  // Do not rebuild if we don't need to.
+  // A rebuild event could be generated, but then
+  // this function could end up being called from somewhere else
+  // before that event gets processed, for example if
+  // a node or callback group is manually added to the executor.
+  const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false);
+  if (!notify_waitable_triggered && !this->collector_.has_pending()) {
+    return;
+  }
 
-  this->refresh_current_collection_from_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_all_callback_groups()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_all_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_manually_added_callback_groups()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_manually_added_callback_groups();
-}
-
-std::vector<rclcpp::CallbackGroup::WeakPtr>
-EventsExecutor::get_automatically_added_callback_groups_from_nodes()
-{
-  this->entities_collector_->update_collections();
-  return this->entities_collector_->get_automatically_added_callback_groups();
-}
-
-void
-EventsExecutor::refresh_current_collection_from_callback_groups()
-{
   // Build the new collection
-  this->entities_collector_->update_collections();
-  auto callback_groups = this->entities_collector_->get_all_callback_groups();
+  this->collector_.update_collections();
+  auto callback_groups = this->collector_.get_all_callback_groups();
   rclcpp::executors::ExecutorEntitiesCollection new_collection;
   rclcpp::executors::build_entities_collection(callback_groups, new_collection);
 
@@ -415,14 +344,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups()
   // We could explicitly check for the notify waitable ID when we receive a waitable event
   // but I think that it's better if the waitable was in the collection and it could be
   // retrieved in the "standard" way.
-  // To do it, we need to add the notify waitable as an entry in both the new and
-  // current collections such that it's neither added or removed.
+  // To do it, we need to add the notify waitable as an entry in the new collection
+  // such that it's neither added or removed (it should have already been added
+  // to the current collection in the constructor)
   this->add_notify_waitable_to_collection(new_collection.waitables);
 
-  // Acquire lock before modifying the current collection
-  std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
-  this->add_notify_waitable_to_collection(current_entities_collection_->waitables);
-
   this->refresh_current_collection(new_collection);
 }
 
@@ -431,14 +357,19 @@ EventsExecutor::refresh_current_collection(
   const rclcpp::executors::ExecutorEntitiesCollection & new_collection)
 {
   // Acquire lock before modifying the current collection
-  std::lock_guard<std::recursive_mutex> lock(collection_mutex_);
+  std::lock_guard<std::mutex> guard(mutex_);
 
-  current_entities_collection_->timers.update(
+  // Remove expired entities to ensure re-initialized objects
+  // are updated. This fixes issues with stale state entities.
+  // See: https://github.com/ros2/rclcpp/pull/2586
+  current_collection_.remove_expired_entities();
+
+  current_collection_.timers.update(
     new_collection.timers,
     [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);},
     [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);});
 
-  current_entities_collection_->subscriptions.update(
+  current_collection_.subscriptions.update(
     new_collection.subscriptions,
     [this](auto subscription) {
       subscription->set_on_new_message_callback(
@@ -447,7 +378,7 @@ EventsExecutor::refresh_current_collection(
     },
     [](auto subscription) {subscription->clear_on_new_message_callback();});
 
-  current_entities_collection_->clients.update(
+  current_collection_.clients.update(
     new_collection.clients,
     [this](auto client) {
       client->set_on_new_response_callback(
@@ -456,7 +387,7 @@ EventsExecutor::refresh_current_collection(
     },
     [](auto client) {client->clear_on_new_response_callback();});
 
-  current_entities_collection_->services.update(
+  current_collection_.services.update(
     new_collection.services,
     [this](auto service) {
       service->set_on_new_request_callback(
@@ -467,12 +398,12 @@ EventsExecutor::refresh_current_collection(
 
   // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS
   /*
-  current_entities_collection_->guard_conditions.update(new_collection.guard_conditions,
+  current_collection_.guard_conditions.update(new_collection.guard_conditions,
     [](auto guard_condition) {(void)guard_condition;},
     [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);});
   */
 
-  current_entities_collection_->waitables.update(
+  current_collection_.waitables.update(
     new_collection.waitables,
     [this](auto waitable) {
       waitable->set_on_ready_callback(

rclcpp/src/rclcpp/generic_client.cpp

@@ -31,22 +31,31 @@ GenericClient::GenericClient(
   rcl_client_options_t & client_options)
 : ClientBase(node_base, node_graph)
 {
-  ts_lib_ = get_typesupport_library(
-    service_type, "rosidl_typesupport_cpp");
+  const rosidl_service_type_support_t * service_ts;
+  try {
+    ts_lib_ = get_typesupport_library(
+      service_type, "rosidl_typesupport_cpp");
 
-  auto service_ts_ = get_service_typesupport_handle(
-    service_type, "rosidl_typesupport_cpp", *ts_lib_);
+    service_ts = get_service_typesupport_handle(
+      service_type, "rosidl_typesupport_cpp", *ts_lib_);
 
-  auto response_type_support_intro = get_message_typesupport_handle(
-    service_ts_->response_typesupport,
-    rosidl_typesupport_introspection_cpp::typesupport_identifier);
-  response_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
-    response_type_support_intro->data);
+    auto response_type_support_intro = get_message_typesupport_handle(
+      service_ts->response_typesupport,
+      rosidl_typesupport_introspection_cpp::typesupport_identifier);
+    response_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
+      response_type_support_intro->data);
+  } catch (std::runtime_error & err) {
+    RCLCPP_ERROR(
+      rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"),
+      "Invalid service type: %s",
+      err.what());
+    throw rclcpp::exceptions::InvalidServiceTypeError(err.what());
+  }
 
   rcl_ret_t ret = rcl_client_init(
     this->get_client_handle().get(),
     this->get_rcl_node_handle(),
-    service_ts_,
+    service_ts,
     service_name.c_str(),
     &client_options);
   if (ret != RCL_RET_OK) {
@@ -100,6 +109,13 @@ GenericClient::handle_response(
   if (std::holds_alternative<Promise>(value)) {
     auto & promise = std::get<Promise>(value);
     promise.set_value(std::move(response));
+  } else if (std::holds_alternative<CallbackTypeValueVariant>(value)) {
+    auto & inner = std::get<CallbackTypeValueVariant>(value);
+    const auto & callback = std::get<CallbackType>(inner);
+    auto & promise = std::get<Promise>(inner);
+    auto & future = std::get<SharedFuture>(inner);
+    promise.set_value(std::move(response));
+    callback(std::move(future));
   }
 }
 
@@ -119,6 +135,18 @@ GenericClient::remove_pending_request(int64_t request_id)
   return pending_requests_.erase(request_id) != 0u;
 }
 
+bool
+GenericClient::remove_pending_request(const FutureAndRequestId & future)
+{
+  return this->remove_pending_request(future.request_id);
+}
+
+bool
+GenericClient::remove_pending_request(const SharedFutureAndRequestId & future)
+{
+  return this->remove_pending_request(future.request_id);
+}
+
 std::optional<GenericClient::CallbackInfoVariant>
 GenericClient::get_and_erase_pending_request(int64_t request_number)
 {

rclcpp/src/rclcpp/generic_service.cpp

@@ -0,0 +1,172 @@
+// Copyright 2024 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 "rclcpp/generic_service.hpp"
+
+namespace rclcpp
+{
+GenericService::GenericService(
+  std::shared_ptr<rcl_node_t> node_handle,
+  const std::string & service_name,
+  const std::string & service_type,
+  GenericServiceCallback any_callback,
+  rcl_service_options_t & service_options)
+: ServiceBase(node_handle),
+  any_callback_(any_callback)
+{
+  const rosidl_service_type_support_t * service_ts;
+  try {
+    ts_lib_ = get_typesupport_library(
+      service_type, "rosidl_typesupport_cpp");
+
+    service_ts = get_service_typesupport_handle(
+      service_type, "rosidl_typesupport_cpp", *ts_lib_);
+
+    auto request_type_support_intro = get_message_typesupport_handle(
+      service_ts->request_typesupport,
+      rosidl_typesupport_introspection_cpp::typesupport_identifier);
+    request_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
+      request_type_support_intro->data);
+
+    auto response_type_support_intro = get_message_typesupport_handle(
+      service_ts->response_typesupport,
+      rosidl_typesupport_introspection_cpp::typesupport_identifier);
+    response_members_ = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(
+      response_type_support_intro->data);
+  } catch (std::runtime_error & err) {
+    RCLCPP_ERROR(
+      rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"),
+      "Invalid service type: %s",
+      err.what());
+    throw rclcpp::exceptions::InvalidServiceTypeError(err.what());
+  }
+
+  // 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)
+    {
+      if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+          "Error in destruction of rcl service handle: %s",
+          rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+      delete service;
+    });
+  *service_handle_.get() = rcl_get_zero_initialized_service();
+
+  rcl_ret_t ret = rcl_service_init(
+    service_handle_.get(),
+    node_handle.get(),
+    service_ts,
+    service_name.c_str(),
+    &service_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+      auto rcl_node_handle = get_rcl_node_handle();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        service_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle),
+        true);
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
+  }
+  TRACETOOLS_TRACEPOINT(
+    rclcpp_service_callback_added,
+    static_cast<const void *>(get_service_handle().get()),
+    static_cast<const void *>(&any_callback_));
+#ifndef TRACETOOLS_DISABLED
+  any_callback_.register_callback_for_tracing();
+#endif
+}
+
+bool
+GenericService::take_request(
+  SharedRequest request_out,
+  rmw_request_id_t & request_id_out)
+{
+  request_out = create_request();
+  return this->take_type_erased_request(request_out.get(), request_id_out);
+}
+
+std::shared_ptr<void>
+GenericService::create_request()
+{
+  Request request = new uint8_t[request_members_->size_of_];
+  request_members_->init_function(request, rosidl_runtime_cpp::MessageInitialization::ZERO);
+  return std::shared_ptr<void>(
+    request,
+    [this](void * p)
+    {
+      request_members_->fini_function(p);
+      delete[] reinterpret_cast<uint8_t *>(p);
+    });
+}
+
+std::shared_ptr<void>
+GenericService::create_response()
+{
+  Response response = new uint8_t[response_members_->size_of_];
+  response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO);
+  return std::shared_ptr<void>(
+    response,
+    [this](void * p)
+    {
+      response_members_->fini_function(p);
+      delete[] reinterpret_cast<uint8_t *>(p);
+    });
+}
+
+std::shared_ptr<rmw_request_id_t>
+GenericService::create_request_header()
+{
+  return std::make_shared<rmw_request_id_t>();
+}
+
+void
+GenericService::handle_request(
+  std::shared_ptr<rmw_request_id_t> request_header,
+  std::shared_ptr<void> request)
+{
+  auto response = any_callback_.dispatch(
+    this->shared_from_this(), request_header, request, create_response());
+  if (response) {
+    send_response(*request_header, response);
+  }
+}
+
+void
+GenericService::send_response(rmw_request_id_t & req_id, SharedResponse & response)
+{
+  rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, response.get());
+
+  if (ret == RCL_RET_TIMEOUT) {
+    RCLCPP_WARN(
+      node_logger_.get_child("rclcpp"),
+      "failed to send response to %s (timeout): %s",
+      this->get_service_name(), rcl_get_error_string().str);
+    rcl_reset_error();
+    return;
+  }
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
+  }
+}
+
+}  //  namespace rclcpp

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -46,8 +46,7 @@ GenericSubscription::handle_message(
           "handle_message is not implemented for GenericSubscription");
 }
 
-void
-GenericSubscription::handle_serialized_message(
+void GenericSubscription::handle_serialized_message(
   const std::shared_ptr<rclcpp::SerializedMessage> & message,
   const rclcpp::MessageInfo & message_info)
 {

rclcpp/src/rclcpp/logger.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <utility>
@@ -54,6 +55,14 @@ get_node_logger(const rcl_node_t * node)
   return rclcpp::get_logger(logger_name);
 }
 
+// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted).
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
 rcpputils::fs::path
 get_logging_directory()
 {
@@ -67,6 +76,26 @@ get_logging_directory()
   allocator.deallocate(log_dir, allocator.state);
   return path;
 }
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+
+std::filesystem::path
+get_log_directory()
+{
+  char * log_dir = NULL;
+  auto allocator = rcutils_get_default_allocator();
+  rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir);
+  if (RCL_LOGGING_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  std::string path{log_dir};
+  allocator.deallocate(log_dir, allocator.state);
+  return path;
+}
 
 Logger
 Logger::get_child(const std::string & suffix)

rclcpp/src/rclcpp/node_options.cpp

@@ -80,6 +80,7 @@ NodeOptions::operator=(const NodeOptions & other)
     this->clock_type_ = other.clock_type_;
     this->clock_qos_ = other.clock_qos_;
     this->use_clock_thread_ = other.use_clock_thread_;
+    this->enable_logger_service_ = other.enable_logger_service_;
     this->parameter_event_qos_ = other.parameter_event_qos_;
     this->rosout_qos_ = other.rosout_qos_;
     this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;

rclcpp/src/rclcpp/publisher_base.cpp

@@ -135,15 +135,28 @@ void
 PublisherBase::bind_event_callbacks(
   const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks)
 {
-  if (event_callbacks.deadline_callback) {
-    this->add_event_handler(
-      event_callbacks.deadline_callback,
-      RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+  try {
+    if (event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        event_callbacks.deadline_callback,
+        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for deadline; not supported");
   }
-  if (event_callbacks.liveliness_callback) {
-    this->add_event_handler(
-      event_callbacks.liveliness_callback,
-      RCL_PUBLISHER_LIVELINESS_LOST);
+
+  try {
+    if (event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        event_callbacks.liveliness_callback,
+        RCL_PUBLISHER_LIVELINESS_LOST);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for liveliness; not supported");
   }
 
   QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb;
@@ -160,9 +173,9 @@ PublisherBase::bind_event_callbacks(
       this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
     }
   } catch (const UnsupportedEventTypeException & /*exc*/) {
-    RCLCPP_DEBUG(
+    RCLCPP_WARN(
       rclcpp::get_logger("rclcpp"),
-      "Failed to add event handler for incompatible qos; wrong callback type");
+      "Failed to add event handler for incompatible qos; not supported");
   }
 
   IncompatibleTypeCallbackType incompatible_type_cb;
@@ -179,14 +192,21 @@ PublisherBase::bind_event_callbacks(
       this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE);
     }
   } catch (UnsupportedEventTypeException & /*exc*/) {
-    RCLCPP_DEBUG(
+    RCLCPP_WARN(
       rclcpp::get_logger("rclcpp"),
-      "Failed to add event handler for incompatible type; wrong callback type");
+      "Failed to add event handler for incompatible type; not supported");
   }
-  if (event_callbacks.matched_callback) {
-    this->add_event_handler(
-      event_callbacks.matched_callback,
-      RCL_PUBLISHER_MATCHED);
+
+  try {
+    if (event_callbacks.matched_callback) {
+      this->add_event_handler(
+        event_callbacks.matched_callback,
+        RCL_PUBLISHER_MATCHED);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for matched; not supported");
   }
 }
 

rclcpp/src/rclcpp/rate.cpp

@@ -71,12 +71,6 @@ Rate::sleep()
   return true;
 }
 
-bool
-Rate::is_steady() const
-{
-  return clock_->get_clock_type() == RCL_STEADY_TIME;
-}
-
 rcl_clock_type_t
 Rate::get_type() const
 {

rclcpp/src/rclcpp/subscription_base.cpp

@@ -112,16 +112,28 @@ void
 SubscriptionBase::bind_event_callbacks(
   const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks)
 {
-  if (event_callbacks.deadline_callback) {
-    this->add_event_handler(
-      event_callbacks.deadline_callback,
-      RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
+  try {
+    if (event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        event_callbacks.deadline_callback,
+        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for deadline; not supported");
   }
 
-  if (event_callbacks.liveliness_callback) {
-    this->add_event_handler(
-      event_callbacks.liveliness_callback,
-      RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
+  try {
+    if (event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        event_callbacks.liveliness_callback,
+        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for liveliness; not supported");
   }
 
   QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb;
@@ -139,7 +151,9 @@ SubscriptionBase::bind_event_callbacks(
       this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
     }
   } catch (const UnsupportedEventTypeException & /*exc*/) {
-    // pass
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible qos; not supported");
   }
 
   IncompatibleTypeCallbackType incompatible_type_cb;
@@ -156,18 +170,33 @@ SubscriptionBase::bind_event_callbacks(
       this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE);
     }
   } catch (UnsupportedEventTypeException & /*exc*/) {
-    // pass
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for incompatible type; not supported");
   }
 
-  if (event_callbacks.message_lost_callback) {
-    this->add_event_handler(
-      event_callbacks.message_lost_callback,
-      RCL_SUBSCRIPTION_MESSAGE_LOST);
+  try {
+    if (event_callbacks.message_lost_callback) {
+      this->add_event_handler(
+        event_callbacks.message_lost_callback,
+        RCL_SUBSCRIPTION_MESSAGE_LOST);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for message lost; not supported");
   }
-  if (event_callbacks.matched_callback) {
-    this->add_event_handler(
-      event_callbacks.matched_callback,
-      RCL_SUBSCRIPTION_MATCHED);
+
+  try {
+    if (event_callbacks.matched_callback) {
+      this->add_event_handler(
+        event_callbacks.matched_callback,
+        RCL_SUBSCRIPTION_MATCHED);
+    }
+  } catch (const UnsupportedEventTypeException & /*exc*/) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to add event handler for matched; not supported");
   }
 }
 
@@ -544,9 +573,30 @@ SubscriptionBase::get_content_filter() const
 // DYNAMIC TYPE ==================================================================================
 bool
 SubscriptionBase::take_dynamic_message(
-  rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/,
-  rclcpp::MessageInfo & /*message_info_out*/)
+  rclcpp::dynamic_typesupport::DynamicMessage & message_out,
+  rclcpp::MessageInfo & message_info_out)
 {
-  throw std::runtime_error("Unimplemented");
-  return false;
+  if (rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE)) {
+    rcl_ret_t ret = rcl_take_dynamic_message(
+      this->get_subscription_handle().get(),
+      &message_out.get_rosidl_dynamic_data(),
+      &message_info_out.get_rmw_message_info(),
+      nullptr);
+    if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
+      return false;
+    } else if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Error when taking dynamic message");
+    }
+  } else {  // Fall back to serialized conversion if direct dynamic message taking isn't supported
+    std::shared_ptr<rclcpp::SerializedMessage> serialized_msg = this->create_serialized_message();
+    if (!this->take_serialized(*serialized_msg.get(), message_info_out)) {
+      std::runtime_error("Couldn't take serialized message when attempting to take dynamic data!");
+    }
+    bool ret = message_out.deserialize(serialized_msg->get_rcl_serialized_message());
+    if (!ret) {
+      throw std::runtime_error("Couldn't convert serialized message to dynamic data!");
+    }
+    this->return_serialized_message(serialized_msg);
+  }
+  return true;
 }

rclcpp/src/rclcpp/time.cpp

@@ -60,6 +60,10 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
 Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
 : rcl_time_(init_time_point(clock_type))
 {
+  if (nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
   rcl_time_.nanoseconds = nanoseconds;
 }
 
@@ -249,6 +253,9 @@ Time::operator+=(const rclcpp::Duration & rhs)
   }
 
   rcl_time_.nanoseconds += rhs.nanoseconds();
+  if (rcl_time_.nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
 
   return *this;
 }
@@ -264,6 +271,9 @@ Time::operator-=(const rclcpp::Duration & rhs)
   }
 
   rcl_time_.nanoseconds -= rhs.nanoseconds();
+  if (rcl_time_.nanoseconds < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
 
   return *this;
 }

rclcpp/src/rclcpp/waitable.cpp

@@ -54,114 +54,8 @@ Waitable::get_number_of_ready_guard_conditions()
   return 0u;
 }
 
-std::shared_ptr<void>
-Waitable::take_data_by_entity_id(size_t id)
-{
-  (void)id;
-  throw std::runtime_error(
-          "Custom waitables should override take_data_by_entity_id "
-          "if they want to use it.");
-}
-
 bool
 Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
-
-void
-Waitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
-{
-  (void)callback;
-
-  throw std::runtime_error(
-          "Custom waitables should override set_on_ready_callback "
-          "if they want to use it.");
-}
-
-void
-Waitable::clear_on_ready_callback()
-{
-  throw std::runtime_error(
-          "Custom waitables should override clear_on_ready_callback if they "
-          "want to use it and make sure to call it on the waitable destructor.");
-}
-
-void
-Waitable::add_to_wait_set(rcl_wait_set_t * wait_set)
-{
-  this->add_to_wait_set(*wait_set);
-}
-
-void
-Waitable::add_to_wait_set(rcl_wait_set_t & wait_set)
-{
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  this->add_to_wait_set(&wait_set);
-// remove warning suppression
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-}
-
-bool
-Waitable::is_ready(rcl_wait_set_t * wait_set)
-{
-  const rcl_wait_set_t & const_wait_set_ref = *wait_set;
-  return this->is_ready(const_wait_set_ref);
-}
-
-bool
-Waitable::is_ready(const rcl_wait_set_t & wait_set)
-{
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  // note this const cast is only required to support a deprecated function
-  return this->is_ready(&const_cast<rcl_wait_set_t &>(wait_set));
-// remove warning suppression
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-}
-
-void
-Waitable::execute(std::shared_ptr<void> & data)
-{
-  const std::shared_ptr<void> & const_data = data;
-  this->execute(const_data);
-}
-
-void
-Waitable::execute(const std::shared_ptr<void> & data)
-{
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-  // note this const cast is only required to support a deprecated function
-  this->execute(const_cast<std::shared_ptr<void> &>(data));
-// remove warning suppression
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-}

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -189,71 +189,6 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be
   }
 }
 
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_add_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    (void)_;
-    executor.add_node(node);
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_remove_node)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  for (auto _ : st) {
-    (void)_;
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-    executor.remove_node(node);
-  }
-}
-
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  auto ret = executor.spin_until_future_complete(shared_future, 100ms);
-  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-    st.SkipWithError(rcutils_get_error_string().str);
-  }
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    (void)_;
-    // static_single_thread_executor has a special design. We need to add/remove the node each
-    // time you call spin
-    st.PauseTiming();
-    executor.add_node(node);
-    st.ResumeTiming();
-
-    ret = executor.spin_until_future_complete(shared_future, 100ms);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-    st.PauseTiming();
-    executor.remove_node(node);
-    st.ResumeTiming();
-  }
-}
-
 BENCHMARK_F(
   PerformanceTestExecutorSimple,
   single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
@@ -314,30 +249,6 @@ BENCHMARK_F(
   }
 }
 
-BENCHMARK_F(
-  PerformanceTestExecutorSimple,
-  static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
-{
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
-  // test success of an immediately finishing future
-  std::promise<bool> promise;
-  std::future<bool> future = promise.get_future();
-  promise.set_value(true);
-  auto shared_future = future.share();
-
-  reset_heap_counters();
-
-  for (auto _ : st) {
-    (void)_;
-    auto ret = rclcpp::executors::spin_node_until_future_complete(
-      executor, node, shared_future, 1s);
-    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
-      st.SkipWithError(rcutils_get_error_string().str);
-      break;
-    }
-  }
-}
-
 BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st)
 {
   // test success of an immediately finishing future

rclcpp/test/rclcpp/CMakeLists.txt

@@ -31,6 +31,7 @@ endif()
 ament_add_gtest(
   test_exceptions
   exceptions/test_exceptions.cpp)
+ament_add_test_label(test_exceptions mimick)
 if(TARGET test_exceptions)
   target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
 endif()
@@ -52,10 +53,12 @@ if(TARGET test_any_subscription_callback)
   target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_client test_client.cpp)
+ament_add_test_label(test_client mimick)
 if(TARGET test_client)
   target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_clock test_clock.cpp)
+ament_add_test_label(test_clock mimick)
 if(TARGET test_clock)
   target_link_libraries(test_clock ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS})
 endif()
@@ -69,6 +72,7 @@ if(TARGET test_create_timer)
   target_include_directories(test_create_timer PRIVATE ./)
 endif()
 ament_add_gtest(test_generic_client test_generic_client.cpp)
+ament_add_test_label(test_generic_client mimick)
 if(TARGET test_generic_client)
   target_link_libraries(test_generic_client ${PROJECT_NAME}
     mimick
@@ -79,7 +83,20 @@ if(TARGET test_generic_client)
     ${test_msgs_TARGETS}
   )
 endif()
+ament_add_gtest(test_generic_service test_generic_service.cpp)
+ament_add_test_label(test_generic_service mimick)
+if(TARGET test_generic_service)
+  target_link_libraries(test_generic_service ${PROJECT_NAME}
+    mimick
+    ${rcl_interfaces_TARGETS}
+    rmw::rmw
+    rosidl_runtime_cpp::rosidl_runtime_cpp
+    rosidl_typesupport_cpp::rosidl_typesupport_cpp
+    ${test_msgs_TARGETS}
+  )
+endif()
 ament_add_gtest(test_client_common test_client_common.cpp)
+ament_add_test_label(test_client_common mimick)
 if(TARGET test_client_common)
   target_link_libraries(test_client_common ${PROJECT_NAME}
     mimick
@@ -94,18 +111,8 @@ ament_add_gtest(test_create_subscription test_create_subscription.cpp)
 if(TARGET test_create_subscription)
   target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
-function(test_add_callback_groups_to_executor_for_rmw_implementation)
-  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
-  ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp
-    ENV ${rmw_implementation_env_var}
-    TIMEOUT 120
-  )
-  if(TARGET test_add_callback_groups_to_executor${target_suffix})
-    target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME} ${test_msgs_TARGETS})
-  endif()
-endfunction()
-call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation)
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
+ament_add_test_label(test_expand_topic_or_service_name mimick)
 if(TARGET test_expand_topic_or_service_name)
   target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw)
 endif()
@@ -137,6 +144,7 @@ if(TARGET test_intra_process_buffer)
 endif()
 
 ament_add_gtest(test_loaned_message test_loaned_message.cpp)
+ament_add_test_label(test_loaned_message mimick)
 target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 
 ament_add_gtest(test_memory_strategy test_memory_strategy.cpp)
@@ -146,6 +154,7 @@ ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp)
 target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS})
 
 ament_add_gtest(test_node test_node.cpp TIMEOUT 240)
+ament_add_test_label(test_node mimick)
 if(TARGET test_node)
   target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS})
 endif()
@@ -157,6 +166,7 @@ if(TARGET test_node_interfaces__get_node_interfaces)
 endif()
 ament_add_gtest(test_node_interfaces__node_base
   node_interfaces/test_node_base.cpp)
+ament_add_test_label(test_node_interfaces__node_base mimick)
 if(TARGET test_node_interfaces__node_base)
   target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw)
 endif()
@@ -168,6 +178,7 @@ endif()
 ament_add_gtest(test_node_interfaces__node_graph
   node_interfaces/test_node_graph.cpp
   TIMEOUT 120)
+ament_add_test_label(test_node_interfaces__node_graph mimick)
 if(TARGET test_node_interfaces__node_graph)
   target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS})
 endif()
@@ -178,21 +189,25 @@ if(TARGET test_node_interfaces__node_interfaces)
 endif()
 ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
+ament_add_test_label(test_node_interfaces__node_parameters mimick)
 if(TARGET test_node_interfaces__node_parameters)
   target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils)
 endif()
 ament_add_gtest(test_node_interfaces__node_services
   node_interfaces/test_node_services.cpp)
+ament_add_test_label(test_node_interfaces__node_services mimick)
 if(TARGET test_node_interfaces__node_services)
   target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_node_interfaces__node_timers
   node_interfaces/test_node_timers.cpp)
+ament_add_test_label(test_node_interfaces__node_timers mimick)
 if(TARGET test_node_interfaces__node_timers)
   target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_node_interfaces__node_topics
   node_interfaces/test_node_topics.cpp)
+ament_add_test_label(test_node_interfaces__node_topics mimick)
 if(TARGET test_node_interfaces__node_topics)
   target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS})
 endif()
@@ -203,6 +218,7 @@ if(TARGET test_node_interfaces__node_type_descriptions)
 endif()
 ament_add_gtest(test_node_interfaces__node_waitables
   node_interfaces/test_node_waitables.cpp)
+ament_add_test_label(test_node_interfaces__node_waitables mimick)
 if(TARGET test_node_interfaces__node_waitables)
   target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -238,10 +254,12 @@ if(TARGET test_node_global_args)
   target_link_libraries(test_node_global_args ${PROJECT_NAME})
 endif()
 ament_add_gtest(test_node_options test_node_options.cpp)
+ament_add_test_label(test_node_options mimick)
 if(TARGET test_node_options)
   target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl)
 endif()
 ament_add_gtest(test_init_options test_init_options.cpp)
+ament_add_test_label(test_init_options mimick)
 if(TARGET test_init_options)
   target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -270,6 +288,7 @@ if(TARGET test_parameter_map)
   target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils)
 endif()
 ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120)
+ament_add_test_label(test_publisher mimick)
 if(TARGET test_publisher)
   target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS})
 endif()
@@ -319,27 +338,6 @@ if(TARGET test_qos)
     rmw::rmw
   )
 endif()
-function(test_generic_pubsub_for_rmw_implementation)
-  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
-  ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp
-    ENV ${rmw_implementation_env_var}
-  )
-  if(TARGET test_generic_pubsub${target_suffix})
-    target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS})
-  endif()
-endfunction()
-call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation)
-
-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}
-  )
-  if(TARGET test_qos_event${target_suffix})
-    target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS})
-  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)
@@ -362,15 +360,18 @@ if(TARGET test_serialized_message)
   target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_service test_service.cpp)
+ament_add_test_label(test_service mimick)
 if(TARGET test_service)
   target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS})
 endif()
 ament_add_gmock(test_service_introspection test_service_introspection.cpp)
+ament_add_test_label(test_service_introspection mimick)
 if(TARGET test_service_introspection)
   target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS})
 endif()
 # Creating and destroying nodes is slow with Connext, so this needs larger timeout.
 ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
+ament_add_test_label(test_subscription mimick)
 if(TARGET test_subscription)
   target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -421,6 +422,7 @@ endif()
 
 ament_add_gtest(test_timer test_timer.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_timer mimick)
 if(TARGET test_timer)
   target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -439,6 +441,7 @@ endif()
 
 ament_add_gtest(test_utilities test_utilities.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_utilities mimick)
 if(TARGET test_utilities)
   target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl)
 endif()
@@ -459,11 +462,22 @@ if(TARGET test_interface_traits)
   target_link_libraries(test_interface_traits ${PROJECT_NAME})
 endif()
 
+ament_add_gtest(test_reinitialized_timers
+  executors/test_reinitialized_timers.cpp
+  TIMEOUT 30)
+if(TARGET test_reinitialized_timers)
+  target_link_libraries(test_reinitialized_timers ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(
   test_executors
   executors/test_executors.cpp
+  executors/test_waitable.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 180)
+if(CMAKE_BUILD_TYPE STREQUAL "Debug" AND MSVC)
+  target_compile_options(test_executors PRIVATE "/bigobj")
+endif()
 if(TARGET test_executors)
   target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS})
 endif()
@@ -473,7 +487,7 @@ ament_add_gtest(
   executors/test_executors_timer_cancel_behavior.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 180)
-if(TARGET test_executors)
+if(TARGET test_executors_timer_cancel_behavior)
   target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS})
 endif()
 
@@ -482,7 +496,7 @@ ament_add_gtest(
   executors/test_executors_callback_group_behavior.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 180)
-if(TARGET test_executors)
+if(TARGET test_executors_callback_group_behavior)
   target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME})
 endif()
 
@@ -491,12 +505,33 @@ ament_add_gtest(
   executors/test_executors_intraprocess.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 180)
-if(TARGET test_executors)
+if(TARGET test_executors_intraprocess)
   target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
 
+ament_add_gtest(
+  test_executors_busy_waiting
+  executors/test_executors_busy_waiting.cpp
+  executors/test_waitable.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors_busy_waiting)
+  target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(
+  test_executors_warmup
+  executors/test_executors_warmup.cpp
+  executors/test_waitable.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors_warmup)
+  target_link_libraries(test_executors_warmup ${PROJECT_NAME} ${test_msgs_TARGETS})
+endif()
+
 ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_static_single_threaded_executor mimick)
 if(TARGET test_static_single_threaded_executor)
   target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -515,11 +550,12 @@ endif()
 
 ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+ament_add_test_label(test_executor_notify_waitable mimick)
 if(TARGET test_executor_notify_waitable)
   target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils)
 endif()
 
-ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5)
+ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 60)
 if(TARGET test_events_executor)
   target_link_libraries(test_events_executor ${PROJECT_NAME} ${test_msgs_TARGETS})
 endif()
@@ -532,6 +568,7 @@ endif()
 
 ament_add_gtest(test_guard_condition test_guard_condition.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}")
+ament_add_test_label(test_guard_condition mimick)
 if(TARGET test_guard_condition)
   target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
 endif()
@@ -565,6 +602,7 @@ if(TARGET test_dynamic_storage)
 endif()
 
 ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp)
+ament_add_test_label(test_storage_policy_common mimick)
 if(TARGET test_storage_policy_common)
   target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
@@ -597,24 +635,62 @@ endif()
 ament_add_gtest(test_executor test_executor.cpp
   APPEND_LIBRARY_DIRS "${append_library_dirs}"
   TIMEOUT 120)
+ament_add_test_label(test_executor mimick)
 if(TARGET test_executor)
-  target_link_libraries(test_executor ${PROJECT_NAME} mimick)
+  target_link_libraries(test_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
 endif()
 
 ament_add_gtest(test_graph_listener test_graph_listener.cpp)
+ament_add_test_label(test_graph_listener mimick)
 if(TARGET test_graph_listener)
   target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
 endif()
 
-function(test_subscription_content_filter_for_rmw_implementation)
+ament_add_gmock_executable(test_qos_event test_qos_event.cpp)
+if(TARGET test_qos_event)
+  target_link_libraries(test_qos_event ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS})
+endif()
+
+ament_add_gmock_executable(test_generic_pubsub test_generic_pubsub.cpp)
+if(TARGET test_generic_pubsub)
+  target_link_libraries(test_generic_pubsub ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS})
+endif()
+
+ament_add_gmock_executable(test_add_callback_groups_to_executor test_add_callback_groups_to_executor.cpp)
+if(TARGET test_add_callback_groups_to_executor)
+  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME} ${test_msgs_TARGETS})
+endif()
+
+ament_add_gmock_executable(test_subscription_content_filter test_subscription_content_filter.cpp)
+if(TARGET test_subscription_content_filter)
+  target_link_libraries(test_subscription_content_filter ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
+endif()
+
+function(test_on_all_rmws)
   set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
-  ament_add_gmock(test_subscription_content_filter${target_suffix}
-    test_subscription_content_filter.cpp
+
+  ament_add_gmock_test(test_qos_event
+    TEST_NAME test_qos_event${target_suffix}
+    ENV ${rmw_implementation_env_var}
+  )
+  ament_add_test_label(test_qos_event${target_suffix} mimick)
+
+  ament_add_gmock_test(test_generic_pubsub
+    TEST_NAME test_generic_pubsub${target_suffix}
+    ENV ${rmw_implementation_env_var}
+  )
+
+  ament_add_gmock_test(test_add_callback_groups_to_executor
+    TEST_NAME test_add_callback_groups_to_executor${target_suffix}
     ENV ${rmw_implementation_env_var}
     TIMEOUT 120
   )
-  if(TARGET test_subscription_content_filter${target_suffix})
-    target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS})
-  endif()
+
+  ament_add_gmock_test(test_subscription_content_filter
+    TEST_NAME test_subscription_content_filter${target_suffix}
+    ENV ${rmw_implementation_env_var}
+    TIMEOUT 120
+  )
+  ament_add_test_label(test_subscription_content_filter${target_suffix} mimick)
 endfunction()
-call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation)
+call_for_each_rmw_implementation(test_on_all_rmws)

rclcpp/test/rclcpp/executors/executor_types.hpp

@@ -25,12 +25,37 @@
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 
+// suppress deprecated StaticSingleThreadedExecutor warning
+// we define an alias that explicitly indicates that this class is deprecated, while avoiding
+// polluting a lot of files the gcc pragmas
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+using DeprecatedStaticSingleThreadedExecutor = rclcpp::executors::StaticSingleThreadedExecutor;
+// remove warning suppression
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
+
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
 using ExecutorTypes =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
   rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::executors::StaticSingleThreadedExecutor,
+  DeprecatedStaticSingleThreadedExecutor,
   rclcpp::experimental::executors::EventsExecutor>;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
 
 class ExecutorTypeNames
 {
@@ -46,10 +71,16 @@ public:
     if (std::is_same<T, rclcpp::executors::MultiThreadedExecutor>()) {
       return "MultiThreadedExecutor";
     }
-
-    if (std::is_same<T, rclcpp::executors::StaticSingleThreadedExecutor>()) {
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+    if (std::is_same<T, DeprecatedStaticSingleThreadedExecutor>()) {
       return "StaticSingleThreadedExecutor";
     }
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
 
     if (std::is_same<T, rclcpp::experimental::executors::EventsExecutor>()) {
       return "EventsExecutor";

rclcpp/test/rclcpp/executors/test_events_executor.cpp

@@ -479,14 +479,21 @@ TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks)
   const auto timeout = std::chrono::seconds(10);
   ex.spin_until_future_complete(log_msgs_future, timeout);
 
-  EXPECT_EQ(
-    "New subscription discovered on topic '/test_topic', requesting incompatible QoS. "
-    "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
-    pub_log_msg);
-  EXPECT_EQ(
-    "New publisher discovered on topic '/test_topic', offering incompatible QoS. "
-    "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
-    sub_log_msg);
+  rclcpp::QoSCheckCompatibleResult qos_compatible = rclcpp::qos_check_compatible(
+    publisher->get_actual_qos(), subscription->get_actual_qos());
+  if (qos_compatible.compatibility == rclcpp::QoSCompatibility::Error) {
+    EXPECT_EQ(
+      "New subscription discovered on topic '/test_topic', requesting incompatible QoS. "
+      "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
+      pub_log_msg);
+    EXPECT_EQ(
+      "New publisher discovered on topic '/test_topic', offering incompatible QoS. "
+      "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
+      sub_log_msg);
+  } else {
+    EXPECT_EQ("", pub_log_msg);
+    EXPECT_EQ("", sub_log_msg);
+  }
 
   rcutils_logging_set_output_handler(original_output_handler);
 }

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -39,8 +39,10 @@
 #include "rclcpp/time_source.hpp"
 
 #include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
 
 #include "./executor_types.hpp"
+#include "./test_waitable.hpp"
 
 using namespace std::chrono_literals;
 
@@ -331,106 +333,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout)
   spinner.join();
 }
 
-class TestWaitable : public rclcpp::Waitable
-{
-public:
-  TestWaitable() = default;
-
-  void
-  add_to_wait_set(rcl_wait_set_t & wait_set) override
-  {
-    if (trigger_count_ > 0) {
-      // Keep the gc triggered until the trigger count is reduced back to zero.
-      // This is necessary if trigger() results in the wait set waking, but not
-      // executing this waitable, in which case it needs to be re-triggered.
-      gc_.trigger();
-    }
-    rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_);
-  }
-
-  void trigger()
-  {
-    trigger_count_++;
-    gc_.trigger();
-  }
-
-  bool
-  is_ready(const rcl_wait_set_t & wait_set) override
-  {
-    for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) {
-      auto rcl_guard_condition = wait_set.guard_conditions[i];
-      if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) {
-        return true;
-      }
-    }
-    return false;
-  }
-
-  std::shared_ptr<void>
-  take_data() override
-  {
-    return nullptr;
-  }
-
-  std::shared_ptr<void>
-  take_data_by_entity_id(size_t id) override
-  {
-    (void) id;
-    return nullptr;
-  }
-
-  void
-  execute(const std::shared_ptr<void> &) override
-  {
-    trigger_count_--;
-    count_++;
-    if (nullptr != on_execute_callback_) {
-      on_execute_callback_();
-    } else {
-      // TODO(wjwwood): I don't know why this was here, but probably it should
-      //   not be there, or test cases where that is important should use the
-      //   on_execute_callback?
-      std::this_thread::sleep_for(3ms);
-    }
-  }
-
-  void
-  set_on_execute_callback(std::function<void()> on_execute_callback)
-  {
-    on_execute_callback_ = on_execute_callback;
-  }
-
-  void
-  set_on_ready_callback(std::function<void(size_t, int)> callback) override
-  {
-    auto gc_callback = [callback](size_t count) {
-        callback(count, 0);
-      };
-    gc_.set_on_trigger_callback(gc_callback);
-  }
-
-  void
-  clear_on_ready_callback() override
-  {
-    gc_.set_on_trigger_callback(nullptr);
-  }
-
-  size_t
-  get_number_of_ready_guard_conditions() override {return 1;}
-
-  size_t
-  get_count() const
-  {
-    return count_;
-  }
-
-private:
-  std::atomic<size_t> trigger_count_ = 0;
-  std::atomic<size_t> count_ = 0;
-  rclcpp::GuardCondition gc_;
-  std::function<void()> on_execute_callback_ = nullptr;
-};
-
 TYPED_TEST(TestExecutors, spinAll)
 {
   using ExecutorType = TypeParam;
@@ -486,7 +388,7 @@ to_nanoseconds_helper(DurationT duration)
 //   - works nominally (it can execute entities)
 //   - it can execute multiple items at once
 //   - it does not wait for work to be available before returning
-TYPED_TEST(TestExecutors, spin_some)
+TYPED_TEST(TestExecutors, spinSome)
 {
   using ExecutorType = TypeParam;
 
@@ -578,20 +480,14 @@ TYPED_TEST(TestExecutors, spin_some)
 
 // The purpose of this test is to check that the ExecutorT.spin_some() method:
 //   - does not continue executing after max_duration has elapsed
-TYPED_TEST(TestExecutors, spin_some_max_duration)
+// TODO(wjwwood): The `StaticSingleThreadedExecutor`
+//   do not properly implement max_duration (it seems), so disable this test
+//   for them in the meantime.
+//   see: https://github.com/ros2/rclcpp/issues/2462
+TYPED_TEST(TestExecutorsStable, spinSomeMaxDuration)
 {
   using ExecutorType = TypeParam;
 
-  // TODO(wjwwood): The `StaticSingleThreadedExecutor`
-  //   do not properly implement max_duration (it seems), so disable this test
-  //   for them in the meantime.
-  //   see: https://github.com/ros2/rclcpp/issues/2462
-  if (
-    std::is_same<ExecutorType, rclcpp::executors::StaticSingleThreadedExecutor>())
-  {
-    GTEST_SKIP();
-  }
-
   // Use an isolated callback group to avoid interference from any housekeeping
   // items that may be in the default callback group of the node.
   constexpr bool automatically_add_to_executor_with_node = false;
@@ -736,7 +632,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted)
 // and b) refreshing the executor collections.
 // The inconsistent state would happen if the event was processed before the collections were
 // finished to be refreshed: the executor would pick up the event but be unable to process it.
-// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional
+// This would leave the `entities_need_rebuild_` flag to true, preventing additional
 // notify waitable events to be pushed.
 // The behavior is observable only under heavy load, so this test spawns several worker
 // threads. Due to the nature of the bug, this test may still succeed even if the
@@ -745,13 +641,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted)
 TYPED_TEST(TestExecutors, testRaceConditionAddNode)
 {
   using ExecutorType = TypeParam;
-  // rmw_connextdds doesn't support events-executor
-  if (
-    std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>() &&
-    std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0)
-  {
-    GTEST_SKIP();
-  }
 
   // Spawn some threads to do some heavy work
   std::atomic<bool> should_cancel = false;
@@ -772,20 +661,20 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode)
   }
 
   // Create an executor
-  auto executor = std::make_shared<ExecutorType>();
+  ExecutorType executor;
   // Start spinning
   auto executor_thread = std::thread(
-    [executor]() {
-      executor->spin();
+    [&executor]() {
+      executor.spin();
     });
   // Add a node to the executor
-  executor->add_node(this->node);
+  executor.add_node(this->node);
 
   // Cancel the executor (make sure that it's already spinning first)
-  while (!executor->is_spinning() && rclcpp::ok()) {
+  while (!executor.is_spinning() && rclcpp::ok()) {
     continue;
   }
-  executor->cancel();
+  executor.cancel();
 
   // Try to join the thread after cancelling the executor
   // This is the "test". We want to make sure that we can still cancel the executor
@@ -799,6 +688,81 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode)
   }
 }
 
+// Check that executors are correctly notified while they are spinning
+// we notify twice to ensure that the notify waitable is still working
+// after the first notification
+TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning)
+{
+  using ExecutorType = TypeParam;
+
+  // Create executor, add the node and start spinning
+  ExecutorType executor;
+  executor.add_node(this->node);
+  std::thread spinner([&]() {executor.spin();});
+
+  // Wait for executor to be spinning
+  while (!executor.is_spinning()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  // Create the first subscription while the executor is already spinning
+  std::atomic<size_t> sub1_msg_count {0};
+  auto sub1 = this->node->template create_subscription<test_msgs::msg::Empty>(
+    this->publisher->get_topic_name(),
+    rclcpp::QoS(10),
+    [&sub1_msg_count](test_msgs::msg::Empty::ConstSharedPtr) {
+      sub1_msg_count++;
+    });
+
+  // Wait for the subscription to be matched
+  size_t tries = 10000;
+  while (this->publisher->get_subscription_count() < 2 && tries-- > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  ASSERT_EQ(this->publisher->get_subscription_count(), 2);
+
+  // Publish a message and verify it's received
+  this->publisher->publish(test_msgs::msg::Empty());
+  auto start = std::chrono::steady_clock::now();
+  while (sub1_msg_count == 0 && (std::chrono::steady_clock::now() - start) < 10s) {
+    std::this_thread::sleep_for(1ms);
+  }
+  EXPECT_EQ(sub1_msg_count, 1u);
+
+  // Create a second subscription while the executor is already spinning
+  std::atomic<size_t> sub2_msg_count {0};
+  auto sub2 = this->node->template create_subscription<test_msgs::msg::Empty>(
+    this->publisher->get_topic_name(),
+    rclcpp::QoS(10),
+    [&sub2_msg_count](test_msgs::msg::Empty::ConstSharedPtr) {
+      sub2_msg_count++;
+    });
+
+  // Wait for the subscription to be matched
+  tries = 10000;
+  while (this->publisher->get_subscription_count() < 3 && tries-- > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  ASSERT_EQ(this->publisher->get_subscription_count(), 3);
+
+  // Publish a message and verify it's received by both subscriptions
+  this->publisher->publish(test_msgs::msg::Empty());
+  start = std::chrono::steady_clock::now();
+  while (
+    sub1_msg_count == 1 &&
+    sub2_msg_count == 0 &&
+    (std::chrono::steady_clock::now() - start) < 10s)
+  {
+    std::this_thread::sleep_for(1ms);
+  }
+  EXPECT_EQ(sub1_msg_count, 2u);
+  EXPECT_EQ(sub2_msg_count, 1u);
+
+  // Cancel needs to be called before join, so that executor.spin() returns.
+  executor.cancel();
+  spinner.join();
+}
+
 // Check spin_until_future_complete with node base pointer (instantiates its own executor)
 TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr)
 {
@@ -869,3 +833,60 @@ TEST(TestExecutors, testSpinWithNonDefaultContext)
 
   rclcpp::shutdown(non_default_context);
 }
+
+TYPED_TEST(TestExecutors, releaseOwnershipEntityAfterSpinningCancel)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  auto future = std::async(std::launch::async, [&executor] {executor.spin();});
+
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {
+    };
+  auto server = node->create_service<test_msgs::srv::Empty>("test_service", callback);
+  while (!executor.is_spinning()) {
+    std::this_thread::sleep_for(50ms);
+  }
+  executor.add_node(node);
+  std::this_thread::sleep_for(50ms);
+  executor.cancel();
+  std::future_status future_status = future.wait_for(1s);
+  EXPECT_EQ(future_status, std::future_status::ready);
+
+  EXPECT_EQ(server.use_count(), 1);
+}
+
+TYPED_TEST(TestExecutors, testRaceDropCallbackGroupFromSecondThread)
+{
+  using ExecutorType = TypeParam;
+
+  // Create an executor
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // Start spinning
+  auto executor_thread = std::thread(
+    [&executor]() {
+      executor.spin();
+    });
+
+  // As the problem is a race, we do this multiple times,
+  // to raise our chances of hitting the problem
+  for (size_t i = 0; i < 10; i++) {
+    auto cg = this->node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+    auto timer = this->node->create_timer(1s, [] {}, cg);
+    // sleep a bit, so that the spin thread can pick up the callback group
+    // and add it to the executor
+    std::this_thread::sleep_for(5ms);
+
+    // At this point the callbackgroup should be used within the waitset of the executor
+    // as we leave the scope, the reference to cg will be dropped.
+    // If the executor has a race, we will experience a segfault at this point.
+  }
+
+  executor.cancel();
+  executor_thread.join();
+}

rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp

@@ -0,0 +1,195 @@
+// Copyright 2024 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 <functional>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "./executor_types.hpp"
+#include "./test_waitable.hpp"
+
+using namespace std::chrono_literals;
+
+template<typename T>
+class TestBusyWaiting : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+
+    const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info();
+    std::stringstream test_name;
+    test_name << test_info->test_case_name() << "_" << test_info->name();
+    node = std::make_shared<rclcpp::Node>("node", test_name.str());
+    callback_group = node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      /* automatically_add_to_executor_with_node =*/ false);
+
+    auto waitable_interfaces = node->get_node_waitables_interface();
+    waitable = std::make_shared<TestWaitable>();
+    waitable_interfaces->add_waitable(waitable, callback_group);
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+
+  void
+  set_up_and_trigger_waitable(std::function<void()> extra_callback = nullptr)
+  {
+    this->has_executed = false;
+    this->waitable->set_on_execute_callback([this, extra_callback]() {
+        if (!this->has_executed) {
+        // trigger once to see if the second trigger is handled or not
+        // this follow up trigger simulates new entities becoming ready while
+        // the executor is executing something else, e.g. subscription got data
+        // or a timer expired, etc.
+        // spin_some would not handle this second trigger, since it collects
+        // work only once, whereas spin_all should handle it since it
+        // collects work multiple times
+          this->waitable->trigger();
+          this->has_executed = true;
+        }
+        if (nullptr != extra_callback) {
+          extra_callback();
+        }
+    });
+    this->waitable->trigger();
+  }
+
+  void
+  check_for_busy_waits(std::chrono::steady_clock::time_point start_time)
+  {
+    // rough time based check, since the work to be done was very small it
+    // should be safe to check that we didn't use more than half the
+    // max duration, which itself is much larger than necessary
+    // however, it could still produce a false-positive
+    EXPECT_LT(
+      std::chrono::steady_clock::now() - start_time,
+      max_duration / 2)
+      << "executor took a long time to execute when it should have done "
+      << "nothing and should not have blocked either, but this could be a "
+      << "false negative if the computer is really slow";
+
+    // this check is making some assumptions about the implementation of the
+    // executors, but it should be safe to say that a busy wait may result in
+    // hundreds or thousands of calls to is_ready(), but "normal" executor
+    // behavior should be within an order of magnitude of the number of
+    // times that the waitable was executed
+    ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count());
+  }
+
+  static constexpr auto max_duration = 10s;
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::CallbackGroup::SharedPtr callback_group;
+  std::shared_ptr<TestWaitable> waitable;
+  std::chrono::steady_clock::time_point start_time;
+  bool has_executed;
+};
+
+TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames);
+
+TYPED_TEST(TestBusyWaiting, test_spin_all)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_callback_group(
+    this->callback_group,
+    this->node->get_node_base_interface());
+
+  this->set_up_and_trigger_waitable();
+
+  auto start_time = std::chrono::steady_clock::now();
+  executor.spin_all(this->max_duration);
+  this->check_for_busy_waits(start_time);
+  // this should get the initial trigger, and the follow up from in the callback
+  ASSERT_EQ(this->waitable->get_count(), 2u);
+}
+
+TYPED_TEST(TestBusyWaiting, test_spin_some)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_callback_group(
+    this->callback_group,
+    this->node->get_node_base_interface());
+
+  this->set_up_and_trigger_waitable();
+
+  auto start_time = std::chrono::steady_clock::now();
+  executor.spin_some(this->max_duration);
+  this->check_for_busy_waits(start_time);
+  // this should get the inital trigger, but not the follow up in the callback
+  ASSERT_EQ(this->waitable->get_count(), 1u);
+}
+
+TYPED_TEST(TestBusyWaiting, test_spin)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_callback_group(
+    this->callback_group,
+    this->node->get_node_base_interface());
+
+  std::condition_variable cv;
+  std::mutex cv_m;
+  bool first_check_passed = false;
+
+  this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() {
+      cv.notify_one();
+      if (!first_check_passed) {
+        std::unique_lock<std::mutex> lk(cv_m);
+        cv.wait_for(lk, 1s, [&]() {return first_check_passed;});
+      }
+  });
+
+  auto start_time = std::chrono::steady_clock::now();
+  std::thread t([&executor]() {
+      executor.spin();
+    });
+
+  // wait until thread has started (first execute of waitable)
+  {
+    std::unique_lock<std::mutex> lk(cv_m);
+    cv.wait_for(lk, 10s);
+  }
+  EXPECT_GT(this->waitable->get_count(), 0u);
+
+  first_check_passed = true;
+  cv.notify_one();
+
+  // wait until the executor has finished (second execute of waitable)
+  {
+    std::unique_lock<std::mutex> lk(cv_m);
+    cv.wait_for(lk, 10s);
+  }
+  EXPECT_EQ(this->waitable->get_count(), 2u);
+
+  executor.cancel();
+  t.join();
+
+  this->check_for_busy_waits(start_time);
+  // this should get the initial trigger, and the follow up from in the callback
+  ASSERT_EQ(this->waitable->get_count(), 2u);
+}

rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp

@@ -282,12 +282,20 @@ public:
   T executor;
 };
 
+#if !defined(_WIN32)
+# ifdef __clang__
+#  pragma clang diagnostic push
+#  pragma clang diagnostic ignored "-Wdeprecated-declarations"
+# endif
+#endif
 using MainExecutorTypes =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
   rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::executors::StaticSingleThreadedExecutor>;
-
+  DeprecatedStaticSingleThreadedExecutor>;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
 // TODO(@fujitatomoya): this test excludes EventExecutor because it does not
 // support simulation time used for this test to relax the racy condition.
 // See more details for https://github.com/ros2/rclcpp/issues/2457.

rclcpp/test/rclcpp/executors/test_executors_warmup.cpp

@@ -0,0 +1,244 @@
+// Copyright 2024 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.
+
+/**
+ * This test checks all implementations of rclcpp::executor to check they pass they basic API
+ * tests. Anything specific to any executor in particular should go in a separate test file.
+ */
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+
+#include "./executor_types.hpp"
+
+using namespace std::chrono_literals;
+
+template<typename T>
+class TestExecutorsWarmup : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TYPED_TEST_SUITE(TestExecutorsWarmup, ExecutorTypes, ExecutorTypeNames);
+
+// This test verifies that spin_all is correctly collecting work multiple times
+// even when one of the items of work is a notifier waitable event and thus results in
+// rebuilding the entities collection.
+// When spin_all goes back to collect more work, it should see the ready items from
+// the new added entities
+TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event
+  auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true);
+  auto node = std::make_shared<rclcpp::Node>("test_node", node_options);
+
+  // Add node to the executor before creating the entities
+  executor.add_node(node);
+
+  // Create entities, this will produce a notifier waitable event, telling the executor to refresh
+  // the entities collection
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("test_topic", rclcpp::QoS(10));
+  size_t callback_count = 0;
+  auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;};
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "test_topic", rclcpp::QoS(10), std::move(callback));
+
+  ASSERT_EQ(callback_count, 0u);
+
+  // Publish a message so that the new entities (i.e. the subscriber) already have work to do
+  publisher->publish(test_msgs::msg::Empty());
+
+  // We need to select a duration that is greater than
+  // the time taken to refresh the entities collection and rebuild the waitset.
+  // spin-all is expected to process the notifier waitable event, rebuild the collection,
+  // and then collect more work, finding the subscription message event.
+  // This duration has been selected empirically.
+  executor.spin_all(std::chrono::milliseconds(500));
+
+  // Verify that the callback is called as part of the spin above
+  EXPECT_EQ(callback_count, 1u);
+}
+
+// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group
+// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589
+TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup_with_cbgroup)
+{
+  using ExecutorType = TypeParam;
+
+  // TODO(alsora): Enable when https://github.com/ros2/rclcpp/pull/2595 gets merged
+  if (
+    std::is_same<ExecutorType, rclcpp::executors::SingleThreadedExecutor>() ||
+    std::is_same<ExecutorType, rclcpp::executors::MultiThreadedExecutor>())
+  {
+    GTEST_SKIP();
+  }
+
+  ExecutorType executor;
+
+  // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event
+  auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true);
+  auto node = std::make_shared<rclcpp::Node>("test_node", node_options);
+
+  auto callback_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+
+  // Add callback group to the executor before creating the entities
+  executor.add_callback_group(callback_group, node->get_node_base_interface());
+
+  // Create entities, this will produce a notifier waitable event, telling the executor to refresh
+  // the entities collection
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("test_topic", rclcpp::QoS(10));
+  size_t callback_count = 0;
+  auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;};
+  rclcpp::SubscriptionOptions sub_options;
+  sub_options.callback_group = callback_group;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "test_topic", rclcpp::QoS(10), std::move(callback), sub_options);
+
+  ASSERT_EQ(callback_count, 0u);
+
+  // Publish a message so that the new entities (i.e. the subscriber) already have work to do
+  publisher->publish(test_msgs::msg::Empty());
+
+  // We need to select a duration that is greater than
+  // the time taken to refresh the entities collection and rebuild the waitset.
+  // spin-all is expected to process the notifier waitable event, rebuild the collection,
+  // and then collect more work, finding the subscription message event.
+  // This duration has been selected empirically.
+  executor.spin_all(std::chrono::milliseconds(500));
+
+  // Verify that the callback is called as part of the spin above
+  EXPECT_EQ(callback_count, 1u);
+}
+
+TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup)
+{
+  using ExecutorType = TypeParam;
+
+  // TODO(alsora): currently only the events-executor passes this test.
+  // Enable single-threaded and multi-threaded executors
+  // when https://github.com/ros2/rclcpp/pull/2595 gets merged
+  if (
+    !std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>())
+  {
+    GTEST_SKIP();
+  }
+
+  ExecutorType executor;
+
+  // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event
+  auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true);
+  auto node = std::make_shared<rclcpp::Node>("test_node", node_options);
+
+  // Add node to the executor before creating the entities
+  executor.add_node(node);
+
+  // Create entities, this will produce a notifier waitable event, telling the executor to refresh
+  // the entities collection
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("test_topic", rclcpp::QoS(10));
+  size_t callback_count = 0;
+  auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;};
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "test_topic", rclcpp::QoS(10), std::move(callback));
+
+  ASSERT_EQ(callback_count, 0u);
+
+  // Publish a message so that the new entities (i.e. the subscriber) already have work to do
+  publisher->publish(test_msgs::msg::Empty());
+
+  // NOTE: intra-process communication is enabled, so the subscription will immediately see
+  // the new message, no risk of race conditions where spin_some gets called before the
+  // message has been delivered.
+  executor.spin_some();
+
+  // Verify that the callback is called as part of the spin above
+  EXPECT_EQ(callback_count, 1u);
+}
+
+TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup_with_cbgroup)
+{
+  using ExecutorType = TypeParam;
+
+  // TODO(alsora): currently only the events-executor passes this test.
+  // Enable single-threaded and multi-threaded executors
+  // when https://github.com/ros2/rclcpp/pull/2595 gets merged
+  if (
+    !std::is_same<ExecutorType, rclcpp::experimental::executors::EventsExecutor>())
+  {
+    GTEST_SKIP();
+  }
+
+  ExecutorType executor;
+
+  // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event
+  auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true);
+  auto node = std::make_shared<rclcpp::Node>("test_node", node_options);
+
+  auto callback_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+
+  // Add callback group to the executor before creating the entities
+  executor.add_callback_group(callback_group, node->get_node_base_interface());
+
+  // Create entities, this will produce a notifier waitable event, telling the executor to refresh
+  // the entities collection
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("test_topic", rclcpp::QoS(10));
+  size_t callback_count = 0;
+  auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;};
+  rclcpp::SubscriptionOptions sub_options;
+  sub_options.callback_group = callback_group;
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "test_topic", rclcpp::QoS(10), std::move(callback), sub_options);
+
+  ASSERT_EQ(callback_count, 0u);
+
+  // Publish a message so that the new entities (i.e. the subscriber) already have work to do
+  publisher->publish(test_msgs::msg::Empty());
+
+  // NOTE: intra-process communication is enabled, so the subscription will immediately see
+  // the new message, no risk of race conditions where spin_some gets called before the
+  // message has been delivered.
+  executor.spin_some();
+
+  // Verify that the callback is called as part of the spin above
+  EXPECT_EQ(callback_count, 1u);
+}

rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp

@@ -32,6 +32,11 @@ protected:
   {
     rclcpp::init(0, nullptr);
   }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
 };
 
 constexpr std::chrono::milliseconds PERIOD_MS = 1000ms;

rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp

@@ -0,0 +1,88 @@
+// Copyright 2024 iRobot 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 <chrono>
+#include <cstddef>
+#include <memory>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "./executor_types.hpp"
+
+template<typename ExecutorType>
+class TestTimersLifecycle : public testing::Test
+{
+public:
+  void SetUp() override {rclcpp::init(0, nullptr);}
+
+  void TearDown() override {rclcpp::shutdown();}
+};
+
+TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes, ExecutorTypeNames);
+
+TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object)
+{
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  auto timers_period = std::chrono::milliseconds(50);
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+
+  executor.add_node(node);
+
+  size_t count_1 = 0;
+  auto timer_1 = rclcpp::create_timer(
+    node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;});
+
+  size_t count_2 = 0;
+  auto timer_2 = rclcpp::create_timer(
+    node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;});
+
+  {
+    std::thread executor_thread([&executor]() {executor.spin();});
+
+    while (count_2 < 10u) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    executor.cancel();
+    executor_thread.join();
+
+    EXPECT_GE(count_2, 10u);
+    EXPECT_LE(count_2 - count_1, 1u);
+  }
+
+  count_1 = 0;
+  timer_1 = rclcpp::create_timer(
+    node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;});
+
+  count_2 = 0;
+  timer_2 = rclcpp::create_timer(
+    node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;});
+
+  {
+    std::thread executor_thread([&executor]() {executor.spin();});
+
+    while (count_2 < 10u) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    executor.cancel();
+    executor_thread.join();
+
+    EXPECT_GE(count_2, 10u);
+    EXPECT_LE(count_2 - count_1, 1u);
+  }
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -20,12 +20,13 @@
 #include <stdexcept>
 
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/executors.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp/executors.hpp"
 
 #include "test_msgs/srv/empty.hpp"
 
+#include "./executor_types.hpp"
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
@@ -46,7 +47,15 @@ public:
 };
 
 TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -56,12 +65,20 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.add_callback_group(cb_group, node->get_node_base_interface(), true),
-      std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
+      std::runtime_error("Failed to handle entities update on callback group add: error not set"));
   }
 }
 
 TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
 
   {
@@ -69,12 +86,20 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.add_node(node),
-      std::runtime_error("Failed to trigger guard condition on node add: error not set"));
+      std::runtime_error("Failed to handle entities update on node add: error not set"));
   }
 }
 
 TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -87,12 +112,20 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai
     RCLCPP_EXPECT_THROW_EQ(
       executor.remove_callback_group(cb_group, true),
       std::runtime_error(
-        "Failed to trigger guard condition on callback group remove: error not set"));
+        "Failed to handle entities update on callback group remove: error not set"));
   }
 }
 
 TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
 
   {
@@ -105,7 +138,15 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
 }
 
 TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
 
   executor.add_node(node);
@@ -115,12 +156,20 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
       "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
     RCLCPP_EXPECT_THROW_EQ(
       executor.remove_node(node, true),
-      std::runtime_error("Failed to trigger guard condition on node remove: error not set"));
+      std::runtime_error("Failed to handle entities update on node remove: error not set"));
   }
 }
 
 TEST_F(TestStaticSingleThreadedExecutor, execute_service) {
-  rclcpp::executors::StaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+  DeprecatedStaticSingleThreadedExecutor executor;
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
   auto node = std::make_shared<rclcpp::Node>("node", "ns");
   executor.add_node(node);
 

rclcpp/test/rclcpp/executors/test_waitable.cpp

@@ -0,0 +1,125 @@
+// Copyright 2024 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 <atomic>
+#include <functional>
+#include <memory>
+#include <thread>
+
+#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
+#include "rclcpp/waitable.hpp"
+
+#include "rcl/wait.h"
+
+#include "test_waitable.hpp"
+
+using namespace std::chrono_literals;
+
+void
+TestWaitable::add_to_wait_set(rcl_wait_set_t & wait_set)
+{
+  if (trigger_count_ > 0) {
+    // Keep the gc triggered until the trigger count is reduced back to zero.
+    // This is necessary if trigger() results in the wait set waking, but not
+    // executing this waitable, in which case it needs to be re-triggered.
+    gc_.trigger();
+  }
+  rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_);
+}
+
+void TestWaitable::trigger()
+{
+  trigger_count_++;
+  gc_.trigger();
+}
+
+bool
+TestWaitable::is_ready(const rcl_wait_set_t & wait_set)
+{
+  is_ready_count_++;
+  for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) {
+    auto rcl_guard_condition = wait_set.guard_conditions[i];
+    if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) {
+      return true;
+    }
+  }
+  return false;
+}
+
+std::shared_ptr<void>
+TestWaitable::take_data()
+{
+  return nullptr;
+}
+
+std::shared_ptr<void>
+TestWaitable::take_data_by_entity_id(size_t id)
+{
+  (void) id;
+  return nullptr;
+}
+
+void
+TestWaitable::execute(const std::shared_ptr<void> &)
+{
+  trigger_count_--;
+  count_++;
+  if (nullptr != on_execute_callback_) {
+    on_execute_callback_();
+  } else {
+    // TODO(wjwwood): I don't know why this was here, but probably it should
+    //   not be there, or test cases where that is important should use the
+    //   on_execute_callback?
+    std::this_thread::sleep_for(3ms);
+  }
+}
+
+void
+TestWaitable::set_on_execute_callback(std::function<void()> on_execute_callback)
+{
+  on_execute_callback_ = on_execute_callback;
+}
+
+void
+TestWaitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
+{
+  auto gc_callback = [callback](size_t count) {
+      callback(count, 0);
+    };
+  gc_.set_on_trigger_callback(gc_callback);
+}
+
+void
+TestWaitable::clear_on_ready_callback()
+{
+  gc_.set_on_trigger_callback(nullptr);
+}
+
+size_t
+TestWaitable::get_number_of_ready_guard_conditions()
+{
+  return 1;
+}
+
+size_t
+TestWaitable::get_count() const
+{
+  return count_;
+}
+
+size_t
+TestWaitable::get_is_ready_call_count() const
+{
+  return is_ready_count_;
+}

rclcpp/test/rclcpp/executors/test_waitable.hpp

@@ -0,0 +1,75 @@
+// Copyright 2024 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__EXECUTORS__TEST_WAITABLE_HPP_
+#define RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_
+
+#include <atomic>
+#include <functional>
+#include <memory>
+
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/guard_condition.hpp"
+
+#include "rcl/wait.h"
+
+class TestWaitable : public rclcpp::Waitable
+{
+public:
+  TestWaitable() = default;
+
+  void
+  add_to_wait_set(rcl_wait_set_t & wait_set) override;
+
+  void trigger();
+
+  bool
+  is_ready(const rcl_wait_set_t & wait_set) override;
+
+  std::shared_ptr<void>
+  take_data() override;
+
+  std::shared_ptr<void>
+  take_data_by_entity_id(size_t id) override;
+
+  void
+  execute(const std::shared_ptr<void> &) override;
+
+  void
+  set_on_execute_callback(std::function<void()> on_execute_callback);
+
+  void
+  set_on_ready_callback(std::function<void(size_t, int)> callback) override;
+
+  void
+  clear_on_ready_callback() override;
+
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  size_t
+  get_count() const;
+
+  size_t
+  get_is_ready_call_count() const;
+
+private:
+  std::atomic<size_t> trigger_count_ = 0;
+  std::atomic<size_t> is_ready_count_ = 0;
+  std::atomic<size_t> count_ = 0;
+  rclcpp::GuardCondition gc_;
+  std::function<void()> on_execute_callback_ = nullptr;
+};
+
+#endif  // RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_

rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp

@@ -38,6 +38,7 @@ protected:
   {
     node.reset();
     wrapped_node.reset();
+    rclcpp::shutdown();
   }
 
   static rclcpp::Node::SharedPtr node;

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -47,9 +47,13 @@ constexpr char absolute_namespace[] = "/ns";
 class TestNodeGraph : public ::testing::Test
 {
 public:
-  void SetUp()
+  static void SetUpTestCase()
   {
     rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
     node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
 
     // This dynamic cast is not necessary for the unittests, but instead is used to ensure
@@ -59,7 +63,7 @@ public:
     ASSERT_NE(nullptr, node_graph_);
   }
 
-  void TearDown()
+  static void TearDownTestCase()
   {
     rclcpp::shutdown();
   }

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -21,6 +21,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <vector>
@@ -61,7 +62,7 @@ protected:
   std::shared_ptr<rclcpp::Node> node;
   rclcpp::node_interfaces::NodeParameters * node_parameters;
 
-  rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY};
+  std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY};
 };
 
 TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -31,13 +31,13 @@ public:
   void add_to_wait_set(rcl_wait_set_t &) override {}
   bool is_ready(const rcl_wait_set_t &) override {return false;}
 
-  std::shared_ptr<void>
-  take_data() override
-  {
-    return nullptr;
-  }
-
+  std::shared_ptr<void> take_data() override {return nullptr;}
   void execute(const std::shared_ptr<void> &) override {}
+
+  void set_on_ready_callback(std::function<void(size_t, int)>) override {}
+  void clear_on_ready_callback() override {}
+
+  std::shared_ptr<void> take_data_by_entity_id(size_t) override {return nullptr;}
 };
 
 class TestNodeWaitables : public ::testing::Test

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -51,13 +51,13 @@ public:
     return test_waitable_result;
   }
 
-  std::shared_ptr<void>
-  take_data() override
-  {
-    return nullptr;
-  }
-
+  std::shared_ptr<void> take_data() override {return nullptr;}
   void execute(const std::shared_ptr<void> &) override {}
+
+  void set_on_ready_callback(std::function<void(size_t, int)>) override {}
+  void clear_on_ready_callback() override {}
+
+  std::shared_ptr<void> take_data_by_entity_id(size_t) override {return nullptr;}
 };
 
 struct RclWaitSetSizes

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -23,13 +23,14 @@
 
 #include "rclcpp/node.hpp"
 #include "test_msgs/msg/empty.hpp"
-#include "test_msgs/msg/empty.h"
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "./executors/executor_types.hpp"
+
 using namespace std::chrono_literals;
 
 template<typename T>
@@ -49,48 +50,8 @@ public:
 template<typename T>
 class TestAddCallbackGroupsToExecutorStable : public TestAddCallbackGroupsToExecutor<T> {};
 
-using ExecutorTypes =
-  ::testing::Types<
-  rclcpp::executors::SingleThreadedExecutor,
-  rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::executors::StaticSingleThreadedExecutor,
-  rclcpp::experimental::executors::EventsExecutor>;
-
-class ExecutorTypeNames
-{
-public:
-  template<typename T>
-  static std::string GetName(int idx)
-  {
-    (void)idx;
-    if (std::is_same<T, rclcpp::executors::SingleThreadedExecutor>()) {
-      return "SingleThreadedExecutor";
-    }
-
-    if (std::is_same<T, rclcpp::executors::MultiThreadedExecutor>()) {
-      return "MultiThreadedExecutor";
-    }
-
-    if (std::is_same<T, rclcpp::executors::StaticSingleThreadedExecutor>()) {
-      return "StaticSingleThreadedExecutor";
-    }
-
-    if (std::is_same<T, rclcpp::experimental::executors::EventsExecutor>()) {
-      return "EventsExecutor";
-    }
-
-    return "";
-  }
-};
-
 TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames);
 
-// StaticSingleThreadedExecutor is not included in these tests for now
-using StandardExecutors =
-  ::testing::Types<
-  rclcpp::executors::SingleThreadedExecutor,
-  rclcpp::executors::MultiThreadedExecutor,
-  rclcpp::experimental::executors::EventsExecutor>;
 TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, ExecutorTypeNames);
 
 /*
@@ -216,7 +177,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t
   std::atomic_size_t timer_count {0};
   auto timer_callback = [&executor, &timer_count]() {
       auto cur_timer_count = timer_count++;
-      printf("in timer_callback(%zu)\n", cur_timer_count);
       if (cur_timer_count > 0) {
         executor.cancel();
       }
@@ -345,32 +305,30 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receiv
       received_message_promise.set_value(true);
     };
 
-  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
-  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
-  // to create a timer with a callback run on another executor
-  rclcpp::TimerBase::SharedPtr timer = nullptr;
   std::promise<void> timer_promise;
+  // create a subscription using the 'cb_grp' callback group
+  rclcpp::QoS qos = rclcpp::QoS(1).reliable();
+  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);
+  // create a publisher to send data
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher =
+    node->create_publisher<test_msgs::msg::Empty>("topic_name", qos);
   auto timer_callback =
-    [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() {
-      if (timer) {
-        timer.reset();
+    [&publisher, &timer_promise]() {
+      if (publisher->get_subscription_count() == 0) {
+        // If discovery hasn't happened yet, get out.
+        return;
       }
-
-      // create a subscription using the `cb_grp` callback group
-      rclcpp::QoS qos = rclcpp::QoS(1).reliable();
-      auto options = rclcpp::SubscriptionOptions();
-      options.callback_group = cb_grp;
-      subscription =
-        node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, sub_callback, options);
-      // create a publisher to send data
-      publisher =
-        node->create_publisher<test_msgs::msg::Empty>("topic_name", qos);
       publisher->publish(test_msgs::msg::Empty());
       timer_promise.set_value();
     };
 
+  // Another executor to run the timer with a callback
   ExecutorType timer_executor;
-  timer = node->create_wall_timer(100ms, timer_callback);
+
+  rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(100ms, timer_callback);
   timer_executor.add_node(node);
   auto future = timer_promise.get_future();
   timer_executor.spin_until_future_complete(future);

rclcpp/test/rclcpp/test_any_service_callback.cpp

@@ -24,7 +24,6 @@
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/service.hpp"
 #include "test_msgs/srv/empty.hpp"
-#include "test_msgs/srv/empty.h"
 
 class TestAnyServiceCallback : public ::testing::Test
 {

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -21,7 +21,6 @@
 
 #include "rclcpp/any_subscription_callback.hpp"
 #include "test_msgs/msg/empty.hpp"
-#include "test_msgs/msg/empty.h"
 
 // Type adapter to be used in tests.
 struct MyEmpty {};

rclcpp/test/rclcpp/test_client.cpp

@@ -89,31 +89,10 @@ TEST_F(TestClient, construction_and_destruction) {
   {
     auto client = node->create_client<ListParameters>("service");
   }
-  {
-    // suppress deprecated function warning
-    #if !defined(_WIN32)
-    # pragma GCC diagnostic push
-    # pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-    #else  // !defined(_WIN32)
-    # pragma warning(push)
-    # pragma warning(disable: 4996)
-    #endif
-
-    auto client = node->create_client<ListParameters>(
-      "service", rmw_qos_profile_services_default);
-
-    // remove warning suppression
-    #if !defined(_WIN32)
-    # pragma GCC diagnostic pop
-    #else  // !defined(_WIN32)
-    # pragma warning(pop)
-    #endif
-  }
   {
     auto client = node->create_client<ListParameters>(
       "service", rclcpp::ServicesQoS());
   }
-
   {
     ASSERT_THROW(
     {
@@ -123,27 +102,6 @@ TEST_F(TestClient, construction_and_destruction) {
 }
 
 TEST_F(TestClient, construction_with_free_function) {
-  {
-    auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
-      node->get_node_base_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_services_interface(),
-      "service",
-      rmw_qos_profile_services_default,
-      nullptr);
-  }
-  {
-    ASSERT_THROW(
-    {
-      auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
-        node->get_node_base_interface(),
-        node->get_node_graph_interface(),
-        node->get_node_services_interface(),
-        "invalid_?service",
-        rmw_qos_profile_services_default,
-        nullptr);
-    }, rclcpp::exceptions::InvalidServiceNameError);
-  }
   {
     auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
       node->get_node_base_interface(),

rclcpp/test/rclcpp/test_client_common.cpp

@@ -431,7 +431,7 @@ TYPED_TEST(TestAllClientTypesWithServer, client_qos)
 
   rclcpp::ServicesQoS qos_profile;
   qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic);
-  rclcpp::Duration duration(std::chrono::nanoseconds(1));
+  rclcpp::Duration duration(std::chrono::milliseconds(1));
   qos_profile.deadline(duration);
   qos_profile.lifespan(duration);
   qos_profile.liveliness_lease_duration(duration);

rclcpp/test/rclcpp/test_create_subscription.cpp

@@ -20,7 +20,6 @@
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/node.hpp"
 #include "test_msgs/msg/empty.hpp"
-#include "test_msgs/msg/empty.h"
 
 using namespace std::chrono_literals;
 
@@ -93,3 +92,19 @@ TEST_F(TestCreateSubscription, create_with_statistics) {
   ASSERT_NE(nullptr, subscription);
   EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
 }
+
+TEST_F(TestCreateSubscription, create_with_intra_process_com) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  auto options = rclcpp::SubscriptionOptions();
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+
+  auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
+  ASSERT_NO_THROW(
+  {
+    subscription = rclcpp::create_subscription<test_msgs::msg::Empty>(
+      node, "topic_name", rclcpp::SystemDefaultsQoS(), callback, options);
+  });
+  ASSERT_NE(nullptr, subscription);
+  EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name());
+}

rclcpp/test/rclcpp/test_executor.cpp

@@ -138,7 +138,7 @@ TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
     "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
     dummy.add_callback_group(cb_group, node->get_node_base_interface(), true),
-    std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
+    std::runtime_error("Failed to handle entities update on callback group add: error not set"));
 }
 
 TEST_F(TestExecutor, remove_callback_group_null_node) {
@@ -175,7 +175,7 @@ TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
   RCLCPP_EXPECT_THROW_EQ(
     dummy.remove_callback_group(cb_group, true),
     std::runtime_error(
-      "Failed to trigger guard condition on callback group remove: error not set"));
+      "Failed to handle entities update on callback group remove: error not set"));
 }
 
 TEST_F(TestExecutor, remove_node_not_associated) {

rclcpp/test/rclcpp/test_externally_defined_services.cpp

@@ -34,6 +34,11 @@ protected:
   {
     rclcpp::init(0, nullptr);
   }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
 };
 
 void

rclcpp/test/rclcpp/test_find_weak_nodes.cpp

@@ -27,6 +27,11 @@ protected:
   {
     rclcpp::init(0, nullptr);
   }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
 };
 
 TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {