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

(cherry picked from commit 8cd4d47ec5)

Signed-off-by: solonovamax <solonovamax@12oclockpoint.com>
Co-authored-by: solo <solonovamax@12oclockpoint.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,78 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.18 (2026-02-09)
+--------------------
+* print warning message on owner node if the parameter operation fails. (backport `#3037 <https://github.com/ros2/rclcpp/issues/3037>`_) (`#3040 <https://github.com/ros2/rclcpp/issues/3040>`_)
+* fix context in wait for message wait set (`#3030 <https://github.com/ros2/rclcpp/issues/3030>`_) (`#3033 <https://github.com/ros2/rclcpp/issues/3033>`_)
+* Improve the robustness of the TopicEndpointInfo constructor (backport `#3013 <https://github.com/ros2/rclcpp/issues/3013>`_) (`#3016 <https://github.com/ros2/rclcpp/issues/3016>`_)
+* Contributors: mergify[bot]
+
+16.0.17 (2025-12-23)
+--------------------
+* Unified Node Interfaces: Add const version of get_node_x_interface() (`#3006 <https://github.com/ros2/rclcpp/issues/3006>`_) (`#3010 <https://github.com/ros2/rclcpp/issues/3010>`_)
+* [Humble] Implement Unified Node Interface (NodeInterfaces class) (backport `#2041 <https://github.com/ros2/rclcpp/issues/2041>`_) (`#3002 <https://github.com/ros2/rclcpp/issues/3002>`_)
+* remove I/O from signal handler. (`#3000 <https://github.com/ros2/rclcpp/issues/3000>`_) (`#3005 <https://github.com/ros2/rclcpp/issues/3005>`_)
+* correct test function descriptions (backport `#2970 <https://github.com/ros2/rclcpp/issues/2970>`_) (`#2994 <https://github.com/ros2/rclcpp/issues/2994>`_)
+* Contributors: fabianhirmann, mergify[bot]
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+* Allow for implicitly convertable loggers as well (`#2922 <https://github.com/ros2/rclcpp/issues/2922>`_) (`#2936 <https://github.com/ros2/rclcpp/issues/2936>`_) (`#2938 <https://github.com/ros2/rclcpp/issues/2938>`_)
+* Fix: improve exception context for parameter_value_from (backport `#2917 <https://github.com/ros2/rclcpp/issues/2917>`_) (`#2921 <https://github.com/ros2/rclcpp/issues/2921>`_)
+* Removed warning test_qos (`#2859 <https://github.com/ros2/rclcpp/issues/2859>`_) (`#2925 <https://github.com/ros2/rclcpp/issues/2925>`_)
+* Add qos parameter for wait_for_message function (`#2903 <https://github.com/ros2/rclcpp/issues/2903>`_) (`#2907 <https://github.com/ros2/rclcpp/issues/2907>`_)
+* Contributors: mergify[bot]
+
+16.0.14 (2025-07-16)
+--------------------
+* fix test_publisher_with_system_default_qos. (backport `#2881 <https://github.com/ros2/rclcpp/issues/2881>`_) (`#2882 <https://github.com/ros2/rclcpp/issues/2882>`_)
+* Contributors: mergify[bot]
+
+16.0.13 (2025-06-23)
+--------------------
+* Fix for memory leaks in rclcpp::SerializedMessage (`#2861 <https://github.com/ros2/rclcpp/issues/2861>`_) (`#2865 <https://github.com/ros2/rclcpp/issues/2865>`_)
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* QoSInitialization::from_rmw does not validate invalid history policy … (backport `#2841 <https://github.com/ros2/rclcpp/issues/2841>`_) (`#2844 <https://github.com/ros2/rclcpp/issues/2844>`_)
+* throws std::invalid_argument if ParameterEvent is NULL. (`#2814 <https://github.com/ros2/rclcpp/issues/2814>`_) (`#2824 <https://github.com/ros2/rclcpp/issues/2824>`_)
+* remove redundant typesupport check in serialization module (`#2808 <https://github.com/ros2/rclcpp/issues/2808>`_) (`#2816 <https://github.com/ros2/rclcpp/issues/2816>`_)
+* Contributors: mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+* doc: Added warning to not instantiate Clock directly with RCL_ROS_TIME (`#2768 <https://github.com/ros2/rclcpp/issues/2768>`_) (`#2770 <https://github.com/ros2/rclcpp/issues/2770>`_)
+* apply actual QoS from rmw to the IPC publisher. (backport `#2707 <https://github.com/ros2/rclcpp/issues/2707>`_) (`#2711 <https://github.com/ros2/rclcpp/issues/2711>`_)
+* Adding in topic name to logging on IPC issues (`#2706 <https://github.com/ros2/rclcpp/issues/2706>`_) (`#2709 <https://github.com/ros2/rclcpp/issues/2709>`_)
+* Contributors: mergify[bot]
+
+16.0.11 (2024-11-25)
+--------------------
+* Fix subscription.is_serialized() for callbacks with message info (`#1950 <https://github.com/ros2/rclcpp/issues/1950>`_) (`#2622 <https://github.com/ros2/rclcpp/issues/2622>`_)
+* Use the same context for the specified node in rclcpp::spin functions… (`#2618 <https://github.com/ros2/rclcpp/issues/2618>`_) (`#2620 <https://github.com/ros2/rclcpp/issues/2620>`_)
+* Contributors: mergify[bot], roscan-tech
+
+16.0.10 (2024-07-26)
+--------------------
+* Add test creating two content filter topics with the same topic name (`#2546 <https://github.com/ros2/rclcpp/issues/2546>`_) (`#2549 <https://github.com/ros2/rclcpp/issues/2549>`_) (`#2551 <https://github.com/ros2/rclcpp/issues/2551>`_)
+* Contributors: mergify[bot]
+
+16.0.9 (2024-05-15)
+-------------------
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_) (`#2529 <https://github.com/ros2/rclcpp/issues/2529>`_)
+* address ambiguous auto variable. (`#2481 <https://github.com/ros2/rclcpp/issues/2481>`_) (`#2485 <https://github.com/ros2/rclcpp/issues/2485>`_)
+* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_) (`#2459 <https://github.com/ros2/rclcpp/issues/2459>`_)
+* Contributors: Tamaki Nishino, mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_) (`#2394 <https://github.com/ros2/rclcpp/issues/2394>`_)
+* Contributors: gentoo90
+
 16.0.7 (2023-11-13)
 -------------------
 * Disable the loaned messages inside the executor. (backport `#2335 <https://github.com/ros2/rclcpp/issues/2335>`_) (`#2364 <https://github.com/ros2/rclcpp/issues/2364>`_)

rclcpp/QUALITY_DECLARATION.md

@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 # rclcpp Quality Declaration
 
 The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
@@ -55,7 +55,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d
 
 ### Continuous Integration [2.iv]
 
-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
 
 Currently nightly results can be seen here:
 
@@ -213,7 +213,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab
 
 ## Platform Support [6]
 
-`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
 
 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/)
@@ -225,4 +225,4 @@ Currently nightly build status can be seen here:
 
 ### Vulnerability Disclosure Policy [7.i]
 
-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -950,7 +950,13 @@ public:
       std::holds_alternative<UniquePtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<SharedConstPtrSerializedMessageCallback>(callback_variant_) ||
       std::holds_alternative<ConstRefSharedConstPtrSerializedMessageCallback>(callback_variant_) ||
-      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_);
+      std::holds_alternative<SharedPtrSerializedMessageCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<UniquePtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<SharedConstPtrSerializedMessageWithInfoCallback>(callback_variant_) ||
+      std::holds_alternative<ConstRefSharedConstPtrSerializedMessageWithInfoCallback>(
+      callback_variant_) ||
+      std::holds_alternative<SharedPtrSerializedMessageWithInfoCallback>(callback_variant_);
   }
 
   void

rclcpp/include/rclcpp/client.hpp

@@ -816,7 +816,7 @@ protected:
         "Received invalid sequence number. Ignoring...");
       return std::nullopt;
     }
-    auto value = std::move(it->second.second);
+    std::optional<CallbackInfoVariant> value = std::move(it->second.second);
     this->pending_requests_.erase(request_number);
     return value;
   }

rclcpp/include/rclcpp/clock.hpp

@@ -60,6 +60,13 @@ public:
   /**
    * Initializes the clock instance with the given clock_type.
    *
+   * WARNING Don't instantiate a clock using RCL_ROS_TIME directly,
+   * unless you really know what you are doing. By default no TimeSource
+   * is attached to a new clock. This will lead to the unexpected behavior,
+   * that your RCL_ROS_TIME will run always on system time. If you want
+   * a RCL_ROS_TIME use Node::get_clock(), or make sure to attach a
+   * TimeSource yourself.
+   *
    * \param clock_type type of the clock.
    * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
    */

rclcpp/include/rclcpp/context.hpp

@@ -26,6 +26,7 @@
 #include <unordered_set>
 #include <utility>
 #include <vector>
+#include <stdexcept>
 
 #include "rcl/context.h"
 #include "rcl/guard_condition.h"

rclcpp/include/rclcpp/detail/template_contains.hpp

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

rclcpp/include/rclcpp/detail/template_unique.hpp

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

rclcpp/include/rclcpp/executors.hpp

@@ -107,7 +107,9 @@ spin_until_future_complete(
   const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
-  rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor executor(options);
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -454,6 +454,8 @@ private:
         if (std::next(it) == subscription_ids.end()) {
           // If this is the last subscription, give up ownership
           subscription->provide_intra_process_data(std::move(message));
+          // Last message delivered, break from for loop
+          break;
         } else {
           // Copy the message since we have additional subscriptions to serve
           Deleter deleter = message.get_deleter();
@@ -493,6 +495,8 @@ private:
           if (std::next(it) == subscription_ids.end()) {
             // If this is the last subscription, give up ownership
             ros_message_subscription->provide_intra_process_message(std::move(message));
+            // Last message delivered, break from for loop
+            break;
           } else {
             // Copy the message since we have additional subscriptions to serve
             Deleter deleter = message.get_deleter();

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <stdexcept>
 
+#include "rcl/allocator.h"
 #include "rcl/types.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
@@ -61,7 +62,12 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>();
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
     buffer_allocator_ = std::make_shared<BufferAlloc>();
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -69,7 +75,12 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
     buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      rcutils_allocator_ = rcl_get_default_allocator();
+    } else {
+      rcutils_allocator_ = allocator::get_rcl_allocator<char,
+          BufferAlloc>(*buffer_allocator_.get());
+    }
   }
 
   virtual ~MessageMemoryStrategy() = default;

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

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

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -26,6 +26,7 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -177,4 +178,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,4 +51,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -29,6 +29,7 @@
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -52,12 +53,16 @@ public:
   /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t.
   RCLCPP_PUBLIC
   explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
-  : node_name_(info.node_name),
-    node_namespace_(info.node_namespace),
-    topic_type_(info.topic_type),
-    endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+  : endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
     qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
   {
+    if (!info.node_name || !info.node_namespace || !info.topic_type) {
+      throw std::invalid_argument("Constructor TopicEndpointInfo with invalid topic endpoint info");
+    }
+    node_name_ = info.node_name;
+    node_namespace_ = info.node_namespace;
+    topic_type_ = info.topic_type;
+
     std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
   }
 
@@ -382,4 +387,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -215,4 +216,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -62,4 +63,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,4 +38,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -47,4 +48,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -24,6 +24,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
@@ -97,4 +98,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -45,6 +45,7 @@ public:
    * \param[in] names A list of parameter names of interest.
    * \param[in] types A list of the types of parameter events of iterest.
    *    EventType NEW, DELETED, or CHANGED
+   * \throws std::invalid_argument if event is NULL.
    *
    * Example Usage:
    *

rclcpp/include/rclcpp/publisher.hpp

@@ -177,8 +177,7 @@ public:
     const rclcpp::QoS & qos,
     const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
   {
-    // Topic is unused for now.
-    (void)topic;
+    (void)qos;
     (void)options;
 
     // If needed, setup intra process communication.
@@ -186,19 +185,23 @@ public:
       auto context = node_base->get_context();
       // Get the intra process manager instance for this context.
       auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
-      // Register the publisher with the intra process manager.
-      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
+      // Check if the QoS is compatible with intra-process.
+      auto qos_profile = get_actual_qos();
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication allowed only with keep last history qos policy");
+                "intraprocess communication on topic '" + topic +
+                "' allowed only with keep last history qos policy");
       }
-      if (qos.depth() == 0) {
+      if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with a zero qos history depth value");
+                "intraprocess communication on topic '" + topic +
+                "' is not allowed with a zero qos history depth value");
       }
-      if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) {
+      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }
+      // Register the publisher with the intra process manager.
       uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
       this->setup_intra_process(
         intra_process_publisher_id,

rclcpp/include/rclcpp/publisher_options.hpp

@@ -119,6 +119,10 @@ private:
   rcl_allocator_t
   get_rcl_allocator() const
   {
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    }
+
     if (!plain_allocator_storage_) {
       plain_allocator_storage_ =
         std::make_shared<PlainAllocator>(*this->get_allocator());

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -424,7 +424,11 @@ public:
 
   rcl_allocator_t get_allocator() override
   {
-    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    if constexpr (std::is_same_v<Alloc, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    }
   }
 
   size_t number_of_ready_subscriptions() const override

rclcpp/include/rclcpp/subscription.hpp

@@ -186,11 +186,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");
       }
       if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(

rclcpp/include/rclcpp/subscription_options.hpp

@@ -163,11 +163,15 @@ private:
   rcl_allocator_t
   get_rcl_allocator() const
   {
-    if (!plain_allocator_storage_) {
-      plain_allocator_storage_ =
-        std::make_shared<PlainAllocator>(*this->get_allocator());
+    if constexpr (std::is_same_v<Allocator, std::allocator<void>>) {
+      return rcl_get_default_allocator();
+    } else {
+      if (!plain_allocator_storage_) {
+        plain_allocator_storage_ =
+          std::make_shared<PlainAllocator>(*this->get_allocator());
+      }
+      return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
     }
-    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
   }
 
   // This is a temporal workaround, to make sure that get_allocator()

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
 #define RCLCPP__WAIT_FOR_MESSAGE_HPP_
 
+#include <future>
 #include <memory>
 #include <string>
 
@@ -23,6 +24,7 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/wait_set.hpp"
+#include "rclcpp/qos.hpp"
 
 namespace rclcpp
 {
@@ -54,7 +56,7 @@ bool wait_for_message(
       }
     });
 
-  rclcpp::WaitSet wait_set;
+  rclcpp::WaitSet wait_set({}, {}, {}, {}, {}, {}, context);
   wait_set.add_subscription(subscription);
   RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); );
   wait_set.add_guard_condition(gc);
@@ -79,10 +81,11 @@ bool wait_for_message(
 /**
  * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
  *
- * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[out] out is the message to be filled when a new message is arriving
  * \param[in] node the node pointer to initialize the subscription on.
  * \param[in] topic the topic to wait for messages.
  * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \param[in] qos parameter specifying QoS settings for the subscription.
  * \return true if a message was successfully received, false if message could not
  * be obtained or shutdown was triggered asynchronously on the context.
  */
@@ -91,9 +94,10 @@ bool wait_for_message(
   MsgT & out,
   rclcpp::Node::SharedPtr node,
   const std::string & topic,
-  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1),
+  const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS())
 {
-  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<const MsgT>) {});
+  auto sub = node->create_subscription<MsgT>(topic, qos, [](const std::shared_ptr<const MsgT>) {});
   return wait_for_message<MsgT, Rep, Period>(
     out, sub, node->get_node_options().context(), time_to_wait);
 }

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>16.0.7</version>
+  <version>16.0.18</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>

rclcpp/resource/logging.hpp.em

@@ -125,8 +125,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
-      typename ::rclcpp::Logger>::value, \
+      ::std::is_convertible_v<decltype(logger), ::rclcpp::Logger>, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \
     auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \

rclcpp/src/rclcpp/executors.cpp

@@ -17,7 +17,9 @@
 void
 rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.spin_node_some(node_ptr);
 }
 
@@ -30,7 +32,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr)
 void
 rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
-  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::ExecutorOptions options;
+  options.context = node_ptr->get_context();
+  rclcpp::executors::SingleThreadedExecutor exec(options);
   exec.add_node(node_ptr);
   exec.spin();
   exec.remove_node(node_ptr);

rclcpp/src/rclcpp/logging_mutex.cpp

@@ -14,6 +14,7 @@
 
 #include <memory>
 #include <mutex>
+#include <stdexcept>
 
 #include "rcutils/macros.h"
 

rclcpp/src/rclcpp/parameter_events_filter.cpp

@@ -28,6 +28,9 @@ ParameterEventsFilter::ParameterEventsFilter(
   const std::vector<EventType> & types)
 : event_(event)
 {
+  if (!event) {
+    throw std::invalid_argument("event cannot be null");
+  }
   if (std::find(types.begin(), types.end(), EventType::NEW) != types.end()) {
     for (auto & new_parameter : event->new_parameters) {
       if (std::find(names.begin(), names.end(), new_parameter.name) != names.end()) {

rclcpp/src/rclcpp/parameter_map.cpp

@@ -81,7 +81,15 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod
         throw InvalidParametersException(message);
       }
       const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]);
-      params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
+      ParameterValue value;
+      try {
+        value = parameter_value_from(c_param_value);
+      } catch (const InvalidParameterValueException & e) {
+        throw InvalidParameterValueException(
+                std::string("parameter_value_from failed for parameter '") +
+                c_param_name + "': " + e.what());
+      }
+      params_node.emplace_back(c_param_name, value);
     }
   }
 

rclcpp/src/rclcpp/parameter_service.cpp

@@ -47,7 +47,7 @@ ParameterService::ParameterService(
           response->values.push_back(param.get_value_message());
         }
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);
@@ -68,7 +68,7 @@ ParameterService::ParameterService(
             return static_cast<rclcpp::ParameterType>(type);
           });
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
       }
     },
     qos_profile, nullptr);
@@ -89,7 +89,7 @@ ParameterService::ParameterService(
           result = node_params->set_parameters_atomically(
             {rclcpp::Parameter::from_parameter_msg(p)});
         } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-          RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
+          RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
           result.successful = false;
           result.reason = ex.what();
         }
@@ -117,7 +117,7 @@ ParameterService::ParameterService(
         auto result = node_params->set_parameters_atomically(pvariants);
         response->result = result;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(
+        RCLCPP_WARN(
           rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
         response->result.successful = false;
         response->result.reason = "One or more parameters were not declared before setting";
@@ -137,7 +137,7 @@ ParameterService::ParameterService(
         auto descriptors = node_params->describe_parameters(request->names);
         response->descriptors = descriptors;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
+        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/qos.cpp

@@ -56,8 +56,10 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos)
     case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
     case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
     case RMW_QOS_POLICY_HISTORY_UNKNOWN:
-    default:
       return KeepLast(rmw_qos.depth);
+    default:
+      throw std::invalid_argument(
+              "Invalid history policy enum value passed to QoSInitialization::from_rmw");
   }
 }
 

rclcpp/src/rclcpp/serialization.cpp

@@ -36,7 +36,6 @@ SerializationBase::SerializationBase(const rosidl_message_type_support_t * type_
 void SerializationBase::serialize_message(
   const void * ros_message, SerializedMessage * serialized_message) const
 {
-  rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer.");
   rcpputils::check_true(nullptr != ros_message, "ROS message is nullpointer.");
   rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer.");
 
@@ -52,7 +51,6 @@ void SerializationBase::serialize_message(
 void SerializationBase::deserialize_message(
   const SerializedMessage * serialized_message, void * ros_message) const
 {
-  rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer.");
   rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer.");
   rcpputils::check_true(
     0u != serialized_message->capacity(),

rclcpp/src/rclcpp/serialized_message.cpp

@@ -26,8 +26,13 @@ namespace rclcpp
 
 inline void copy_rcl_message(const rcl_serialized_message_t & from, rcl_serialized_message_t & to)
 {
-  const auto ret = rmw_serialized_message_init(
-    &to, from.buffer_capacity, &from.allocator);
+  auto ret = RCL_RET_ERROR;
+  if (nullptr == to.buffer) {
+    ret = rmw_serialized_message_init(&to, from.buffer_capacity, &from.allocator);
+  } else {
+    ret = rmw_serialized_message_resize(&to, from.buffer_capacity);
+  }
+
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -78,7 +83,6 @@ SerializedMessage::SerializedMessage(rcl_serialized_message_t && other)
 SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other)
 {
   if (this != &other) {
-    serialized_message_ = rmw_get_zero_initialized_serialized_message();
     copy_rcl_message(other.serialized_message_, serialized_message_);
   }
 
@@ -88,7 +92,6 @@ SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other
 SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other)
 {
   if (&serialized_message_ != &other) {
-    serialized_message_ = rmw_get_zero_initialized_serialized_message();
     copy_rcl_message(other, serialized_message_);
   }
 
@@ -98,6 +101,14 @@ SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t
 SerializedMessage & SerializedMessage::operator=(SerializedMessage && other)
 {
   if (this != &other) {
+    if (nullptr != serialized_message_.buffer) {
+      const auto fini_ret = rmw_serialized_message_fini(&serialized_message_);
+      if (RCL_RET_OK != fini_ret) {
+        RCLCPP_ERROR(
+          get_logger("rclcpp"),
+          "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+      }
+    }
     serialized_message_ =
       std::exchange(other.serialized_message_, rmw_get_zero_initialized_serialized_message());
   }
@@ -108,6 +119,14 @@ SerializedMessage & SerializedMessage::operator=(SerializedMessage && other)
 SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && other)
 {
   if (&serialized_message_ != &other) {
+    if (nullptr != serialized_message_.buffer) {
+      const auto fini_ret = rmw_serialized_message_fini(&serialized_message_);
+      if (RCL_RET_OK != fini_ret) {
+        RCLCPP_ERROR(
+          get_logger("rclcpp"),
+          "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+      }
+    }
     serialized_message_ =
       std::exchange(other, rmw_get_zero_initialized_serialized_message());
   }

rclcpp/src/rclcpp/signal_handler.cpp

@@ -68,7 +68,6 @@ void
 SignalHandler::signal_handler(
   int signum, siginfo_t * siginfo, void * context)
 {
-  RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum);
   auto & instance = SignalHandler::get_global_signal_handler();
 
   auto old_signal_handler = instance.get_old_signal_handler(signum);
@@ -91,7 +90,6 @@ SignalHandler::signal_handler(
 void
 SignalHandler::signal_handler(int signum)
 {
-  RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum);
   auto & instance = SignalHandler::get_global_signal_handler();
   auto old_signal_handler = instance.get_old_signal_handler(signum);
   if (
@@ -249,9 +247,6 @@ SignalHandler::signal_handler_common()
 {
   auto & instance = SignalHandler::get_global_signal_handler();
   instance.signal_received_.store(true);
-  RCLCPP_DEBUG(
-    get_logger(),
-    "signal_handler(): notifying deferred signal handler");
   instance.notify_signal_handler();
 }
 
@@ -260,6 +255,7 @@ SignalHandler::deferred_signal_handler()
 {
   while (true) {
     if (signal_received_.exchange(false)) {
+      RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(SIGINT/SIGTERM)");
       RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down");
       for (auto context_ptr : rclcpp::get_contexts()) {
         if (context_ptr->get_init_options().shutdown_on_signal) {

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <utility>

rclcpp/test/rclcpp/CMakeLists.txt

@@ -234,6 +234,11 @@ if(TARGET test_node_interfaces__node_graph)
     "test_msgs")
   target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__node_interfaces
+  node_interfaces/test_node_interfaces.cpp)
+if(TARGET test_node_interfaces__node_interfaces)
+  target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick)
+endif()
 ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
 if(TARGET test_node_interfaces__node_parameters)
@@ -262,6 +267,11 @@ ament_add_gtest(test_node_interfaces__node_waitables
 if(TARGET test_node_interfaces__node_waitables)
   target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__test_template_utils  # Compile time test
+  node_interfaces/detail/test_template_utils.cpp)
+if(TARGET test_node_interfaces__test_template_utils)
+  target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME})
+endif()
 
 # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
 # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -21,6 +21,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <future>
 #include <limits>
 #include <memory>
 #include <string>
@@ -80,7 +81,7 @@ public:
   rclcpp::Node::SharedPtr node;
   rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
   rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
-  int callback_count;
+  std::atomic<int> callback_count;
 };
 
 // spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see:
@@ -178,7 +179,7 @@ TYPED_TEST(TestExecutors, spinWithTimer) {
   using ExecutorType = TypeParam;
   ExecutorType executor;
 
-  bool timer_completed = false;
+  std::atomic<bool> timer_completed = false;
   auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;});
   executor.add_node(this->node);
 
@@ -282,7 +283,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) {
       }
     });
 
-  bool spin_exited = false;
+  std::atomic<bool> spin_exited = false;
 
   // Timeout set to negative for no timeout.
   std::thread spinner([&]() {
@@ -318,7 +319,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) {
   ExecutorType executor;
   executor.add_node(this->node);
 
-  bool spin_exited = false;
+  std::atomic<bool> spin_exited = false;
 
   // Needs to run longer than spin_until_future_complete's timeout.
   std::future<void> future = std::async(
@@ -412,7 +413,7 @@ TYPED_TEST(TestExecutors, spinAll) {
 
   // Long timeout, but should not block test if spin_all works as expected as we cancel the
   // executor.
-  bool spin_exited = false;
+  std::atomic<bool> spin_exited = false;
   std::thread spinner([&spin_exited, &executor, this]() {
       executor.spin_all(1s);
       executor.remove_node(this->node, true);
@@ -518,7 +519,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) {
   ExecutorType executor;
   executor.add_node(this->node);
 
-  bool spin_exited = false;
+  std::atomic<bool> spin_exited = false;
 
   // This needs to block longer than it takes to get to the shutdown call below and for
   // spin_until_future_complete to return
@@ -696,3 +697,31 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) {
   executor.spin();
   EXPECT_EQ(kNumMessages, this->callback_count.load());
 }
+
+// Check spin functions with non default context
+TEST(TestExecutors, testSpinWithNonDefaultContext)
+{
+  auto non_default_context = std::make_shared<rclcpp::Context>();
+  non_default_context->init(0, nullptr);
+
+  {
+    auto node =
+      std::make_unique<rclcpp::Node>("node", rclcpp::NodeOptions().context(non_default_context));
+
+    EXPECT_NO_THROW(rclcpp::spin_some(node->get_node_base_interface()));
+
+    auto check_spin_until_future_complete = [&]() {
+        std::promise<bool> promise;
+        std::future<bool> future = promise.get_future();
+        promise.set_value(true);
+
+        auto shared_future = future.share();
+        auto ret = rclcpp::spin_until_future_complete(
+          node->get_node_base_interface(), shared_future, 1s);
+        EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+      };
+    EXPECT_NO_THROW(check_spin_until_future_complete());
+  }
+
+  rclcpp::shutdown(non_default_context);
+}

rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp

@@ -0,0 +1,56 @@
+// 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 <gtest/gtest.h>
+
+#include "rclcpp/detail/template_contains.hpp"
+#include "rclcpp/detail/template_unique.hpp"
+
+
+TEST(NoOpTests, test_node_interfaces_template_utils) {
+}                                                        // This is just to let gtest work
+
+namespace rclcpp
+{
+namespace detail
+{
+
+// This tests template logic at compile time
+namespace
+{
+
+struct test_template_unique
+{
+  static_assert(template_unique_v<int>, "failed");
+  static_assert(template_unique_v<int, double>, "failed");
+  static_assert(!template_unique_v<int, int>, "failed");
+  static_assert(!template_unique_v<int, double, int>, "failed");
+  static_assert(!template_unique_v<int, int, double>, "failed");
+};
+
+
+struct test_template_contains
+{
+  static_assert(template_contains_v<int, int, double>, "failed");
+  static_assert(template_contains_v<double, int, double>, "failed");
+  static_assert(template_contains_v<int, int>, "failed");
+  static_assert(!template_contains_v<int>, "failed");
+  static_assert(!template_contains_v<int, float>, "failed");
+  static_assert(!template_contains_v<int, float, double>, "failed");
+};
+
+}   // namespace
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp

@@ -0,0 +1,306 @@
+// 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 <gtest/gtest.h>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_interfaces.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+
+class TestNodeInterfaces : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+/*
+   Testing NodeInterfaces construction from nodes.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+
+  // Create a NodeInterfaces for base and graph using a rclcpp::Node.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto node_interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(node);
+  }
+
+  // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni) {
+        auto base_interface = ni.get<NodeBaseInterface>();
+      };
+
+    some_func(node);
+  }
+
+  // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni_with_one) {
+        auto base_interface = ni_with_one.get<NodeBaseInterface>();
+      };
+
+    NodeInterfaces<NodeBaseInterface, NodeGraphInterface> ni_with_two(node);
+
+    some_func(ni_with_two);
+  }
+
+  // Create a NodeInterfaces via aggregation of interfaces in constructor.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto loose_node_base = node->get_node_base_interface();
+    auto loose_node_graph = node->get_node_graph_interface();
+    auto ni = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(
+      loose_node_base,
+      loose_node_graph);
+  }
+}
+
+/*
+   Test construction with all standard rclcpp::node_interfaces::Node*Interfaces.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<
+    rclcpp::node_interfaces::NodeBaseInterface,
+    rclcpp::node_interfaces::NodeClockInterface,
+    rclcpp::node_interfaces::NodeGraphInterface,
+    rclcpp::node_interfaces::NodeLoggingInterface,
+    rclcpp::node_interfaces::NodeTimersInterface,
+    rclcpp::node_interfaces::NodeTopicsInterface,
+    rclcpp::node_interfaces::NodeServicesInterface,
+    rclcpp::node_interfaces::NodeWaitablesInterface,
+    rclcpp::node_interfaces::NodeParametersInterface,
+    rclcpp::node_interfaces::NodeTimeSourceInterface
+    >(node);
+}
+
+/*
+   Testing getters.
+ */
+TEST_F(TestNodeInterfaces, ni_init) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = NodeInterfaces<
+    NodeBaseInterface,
+    NodeClockInterface,
+    NodeGraphInterface,
+    NodeLoggingInterface,
+    NodeTimersInterface,
+    NodeTopicsInterface,
+    NodeServicesInterface,
+    NodeWaitablesInterface,
+    NodeParametersInterface,
+    NodeTimeSourceInterface
+    >(node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}
+
+/*
+   Testing macro'ed getters.
+ */
+TEST_F(TestNodeInterfaces, ni_all_init_non_const) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}
+
+TEST_F(TestNodeInterfaces, ni_all_init_const) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  const auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(*node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -93,6 +93,111 @@ TEST_F(TestAnySubscriptionCallback, construct_destruct) {
   rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc2(allocator);
 }
 
+TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) {
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::unique_ptr<rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::unique_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<const rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](const std::shared_ptr<const rclcpp::SerializedMessage> &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set(
+      [](
+        const std::shared_ptr<const rclcpp::SerializedMessage> &,
+        const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<rclcpp::SerializedMessage>) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+  {
+    rclcpp::AnySubscriptionCallback<test_msgs::msg::Empty> asc;
+    asc.set([](std::shared_ptr<rclcpp::SerializedMessage>, const rclcpp::MessageInfo &) {});
+    EXPECT_TRUE(asc.is_serialized_message_callback());
+    EXPECT_NO_THROW(
+      asc.dispatch(
+        std::make_shared<rclcpp::SerializedMessage>(),
+        rclcpp::MessageInfo{}));
+  }
+}
+
 TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) {
   EXPECT_THROW(
     any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_),

rclcpp/test/rclcpp/test_create_subscription.cpp

@@ -93,3 +93,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_generic_pubsub.cpp

@@ -15,6 +15,7 @@
 
 #include <gmock/gmock.h>
 
+#include <chrono>
 #include <future>
 #include <memory>
 #include <string>

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -430,7 +430,7 @@ TEST(TestIntraProcessManager, add_pub_sub) {
    - Remove the first subscription from ipm and add a new one.
    - Publishes a unique_ptr message with a subscription not requesting ownership.
    - The received message is expected to be the same, the first subscription do not receive it.
-   - Publishes a shared_ptr message with a subscription not requesting ownership.
+   - Publishes a unique_ptr message with a subscription not requesting ownership.
    - The received message is expected to be the same.
  */
 TEST(TestIntraProcessManager, single_subscription) {
@@ -482,9 +482,9 @@ TEST(TestIntraProcessManager, single_subscription) {
    - One is expected to receive the published message, while the other will receive a copy.
    - Publishes a unique_ptr message with 2 subscriptions not requesting ownership.
    - Both received messages are expected to be the same as the published one.
-   - Publishes a shared_ptr message with 2 subscriptions requesting ownership.
+   - Publishes a unique_ptr message with 2 subscriptions requesting ownership.
    - Both received messages are expected to be a copy of the published one.
-   - Publishes a shared_ptr message with 2 subscriptions not requesting ownership.
+   - Publishes a unique_ptr message with 2 subscriptions not requesting ownership.
    - Both received messages are expected to be the same as the published one.
  */
 TEST(TestIntraProcessManager, multiple_subscriptions_same_type) {
@@ -589,9 +589,9 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) {
    - The 2 subscriptions not requesting ownership are expected to both receive the same copy
      of the message, one of the subscription requesting ownership is expected to receive a
      different copy, while the last is expected to receive the published message.
-   - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not.
-   - The subscription requesting ownership is expected to receive a copy of the message, while
-     the other is expected to receive the published message
+   - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not.
+   - The subscription requesting ownership is expected to receive the published message, while
+     the other is expected to receive a copy of the published message
  */
 TEST(TestIntraProcessManager, multiple_subscriptions_different_type) {
   using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager;

rclcpp/test/rclcpp/test_logging.cpp

@@ -253,9 +253,19 @@ bool log_function_const_ref(const rclcpp::Logger & logger)
   return true;
 }
 
+class DerivedLogger : public rclcpp::Logger
+{
+public:
+  explicit DerivedLogger(const rclcpp::Logger & logger)
+  : rclcpp::Logger(logger) {}
+};
+
 TEST_F(TestLoggingMacros, test_log_from_node) {
   auto logger = rclcpp::get_logger("test_logging_logger");
   EXPECT_TRUE(log_function(logger));
   EXPECT_TRUE(log_function_const(logger));
   EXPECT_TRUE(log_function_const_ref(logger));
+
+  DerivedLogger derived_logger(logger);
+  RCLCPP_INFO(derived_logger, "successful log from derived logger");
 }

rclcpp/test/rclcpp/test_parameter_events_filter.cpp

@@ -72,6 +72,13 @@ protected:
 /*
    Testing filters.
  */
+TEST_F(TestParameterEventFilter, invalide_arguments) {
+  EXPECT_THROW(
+    rclcpp::ParameterEventsFilter(nullptr, {"new"}, {nt}),
+    std::invalid_argument
+  );
+}
+
 TEST_F(TestParameterEventFilter, full_by_type) {
   auto res = rclcpp::ParameterEventsFilter(
     full,

rclcpp/test/rclcpp/test_parameter_service.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <utility>

rclcpp/test/rclcpp/test_publisher.cpp

@@ -176,6 +176,23 @@ TEST_F(TestPublisher, various_creation_signatures) {
   }
 }
 
+/*
+   Testing publisher with intraprocess enabled and SystemDefaultQoS
+ */
+TEST_F(TestPublisher, test_publisher_with_system_default_qos) {
+  initialize(rclcpp::NodeOptions().use_intra_process_comms(false));
+  // explicitly enable intra-process comm with publisher option
+  auto options = rclcpp::PublisherOptions();
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+  // intraprocess communication allowed only with volatile durability
+  rclcpp::QoS qos = rclcpp::QoS(10).durability_volatile();
+  using test_msgs::msg::Empty;
+  ASSERT_NO_THROW(
+  {
+    auto publisher = node->create_publisher<Empty>("topic", qos, options);
+  });
+}
+
 /*
    Testing publisher with intraprocess enabled and invalid QoS
  */
@@ -418,11 +435,10 @@ TEST_F(TestPublisher, intra_process_publish_failures) {
       publisher->publish(std::move(loaned_msg)),
       std::runtime_error("loaned message is not valid"));
   }
-  RCLCPP_EXPECT_THROW_EQ(
-    node->create_publisher<test_msgs::msg::Empty>(
-      "topic", rclcpp::QoS(0), options),
-    std::invalid_argument(
-      "intraprocess communication is not allowed with a zero qos history depth value"));
+  // a zero depth with KEEP_LAST doesn't make sense,
+  // this will be interpreted as SystemDefaultQoS by rclcpp.
+  EXPECT_NO_THROW(
+    node->create_publisher<test_msgs::msg::Empty>("topic", rclcpp::QoS(0), options));
 }
 
 TEST_F(TestPublisher, inter_process_publish_failures) {

rclcpp/test/rclcpp/test_qos.cpp

@@ -250,3 +250,16 @@ TEST(TestQoS, qos_check_compatible)
     EXPECT_FALSE(ret.reason.empty());
   }
 }
+
+TEST(TestQoS, from_rmw_validity)
+{
+  rmw_qos_profile_t invalid_qos;
+  memset(&invalid_qos, 0, sizeof(invalid_qos));
+  unsigned int n = 999;
+  memcpy(&invalid_qos.history, &n, sizeof(n));
+
+  EXPECT_THROW(
+  {
+    rclcpp::QoSInitialization::from_rmw(invalid_qos);
+  }, std::invalid_argument);
+}

rclcpp/test/rclcpp/test_service.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <string>
 #include <memory>
 #include <utility>

rclcpp/test/rclcpp/test_subscription_content_filter.cpp

@@ -310,3 +310,25 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil
     }
   }
 }
+
+TEST_F(
+  CLASSNAME(
+    TestContentFilterSubscription,
+    RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) {
+
+  // Create another content filter
+  auto options = rclcpp::SubscriptionOptions();
+
+  std::string filter_expression = "int32_value > %0";
+  std::vector<std::string> expression_parameters = {"4"};
+
+  options.content_filter_options.filter_expression = filter_expression;
+  options.content_filter_options.expression_parameters = expression_parameters;
+
+  auto callback = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub_2 = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "content_filter_topic", qos, callback, options);
+
+  EXPECT_NE(nullptr, sub_2);
+  sub_2.reset();
+}

rclcpp/test/rclcpp/test_wait_for_message.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -107,3 +108,58 @@ TEST(TestUtilities, wait_for_message_twice_one_sub) {
 
   rclcpp::shutdown();
 }
+
+TEST(TestUtilities, wait_for_last_message) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_last_message_node");
+  auto qos = rclcpp::QoS(1).reliable().transient_local();
+
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node->create_publisher<MsgT>("wait_for_last_message_topic", qos);
+  pub->publish(*get_messages_strings()[0]);
+
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_last_message_topic", 5s, qos);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  ASSERT_NO_THROW(wait.get());
+  ASSERT_TRUE(received);
+  EXPECT_EQ(out, *get_messages_strings()[0]);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestUtilities, wait_for_message_custom_context) {
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+
+  auto node_opt = rclcpp::NodeOptions().context(context);
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_custom_context_node", node_opt);
+
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
+
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  for (auto i = 0u; i < 10 && received == false; ++i) {
+    pub->publish(*get_messages_strings()[0]);
+    std::this_thread::sleep_for(1s);
+  }
+  ASSERT_TRUE(received);
+  EXPECT_EQ(out, *get_messages_strings()[0]);
+
+  context->shutdown("test complete");
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,51 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+16.0.18 (2026-02-09)
+--------------------
+* Update exception documentation for goal cancellation in ServerGoalHandle (`#3019 <https://github.com/ros2/rclcpp/issues/3019>`_) (`#3024 <https://github.com/ros2/rclcpp/issues/3024>`_)
+* Contributors: mergify[bot]
+
+16.0.17 (2025-12-23)
+--------------------
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+
+16.0.14 (2025-07-16)
+--------------------
+
+16.0.13 (2025-06-23)
+--------------------
+* Replace std::default_random_engine with std::mt19937 (humble) (`#2847 <https://github.com/ros2/rclcpp/issues/2847>`_)
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* Harden rclcpp_action::convert(). (backport `#2786 <https://github.com/ros2/rclcpp/issues/2786>`_) (`#2788 <https://github.com/ros2/rclcpp/issues/2788>`_)
+* Contributors: keeponoiro, mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+
+16.0.11 (2024-11-25)
+--------------------
+* fix: Fixed race condition in action server between is_ready and take. Backport from iron `#2531 <https://github.com/ros2/rclcpp/issues/2531>`_ (`#2635 <https://github.com/ros2/rclcpp/issues/2635>`_)
+* Contributors: Camilo Camacho
+
+16.0.10 (2024-07-26)
+--------------------
+
+16.0.9 (2024-05-15)
+-------------------
+* Do not generate the exception when action service response timeout. (`#2464 <https://github.com/ros2/rclcpp/issues/2464>`_) (`#2518 <https://github.com/ros2/rclcpp/issues/2518>`_)
+* Contributors: mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+
 16.0.7 (2023-11-13)
 -------------------
 

rclcpp_action/QUALITY_DECLARATION.md

@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 # rclcpp_action Quality Declaration
 
 The package `rclcpp_action` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
@@ -53,7 +53,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d
 
 ### Continuous Integration [2.iv]
 
-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
 
 Currently nightly results can be seen here:
 
@@ -179,7 +179,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github
 
 ## Platform Support [6]
 
-`rclcpp_action` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp_action` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
 
 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_action/)
@@ -191,4 +191,4 @@ Currently nightly build status can be seen here:
 
 ### Vulnerability Disclosure Policy [7.i]
 
-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -200,7 +200,7 @@ public:
    * This is a terminal state, no more methods should be called on a goal handle after this is
    * called.
    *
-   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
+   * \throws rclcpp::exceptions::RCLError If a cancel request for this goal has not been received.
    *
    * \param[in] result_msg the final result to send to clients.
    */

rclcpp_action/include/rclcpp_action/types.hpp

@@ -39,12 +39,22 @@ RCLCPP_ACTION_PUBLIC
 std::string
 to_string(const GoalUUID & goal_id);
 
-// Convert C++ GoalID to rcl_action_goal_info_t
+/// Convert C++ GoalID to rcl_action_goal_info_t
+/**
+ * \param[in] goal_id C++ GoalUUID reference to be converted.
+ * \param[inout] info rcl_action_goal_info_t structure to be filled.
+ * \throws std::runtime_error if info is null.
+ */
 RCLCPP_ACTION_PUBLIC
 void
 convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info);
 
-// Convert rcl_action_goal_info_t to C++ GoalID
+/// Convert rcl_action_goal_info_t to C++ GoalID
+/**
+ * \param[in] info rcl_action_goal_info_t reference to be converted.
+ * \param[inout] goal_id C++ GoalUUID structure to be filled.
+ * \throws std::runtime_error if goal_id is null.
+ */
 RCLCPP_ACTION_PUBLIC
 void
 convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id);

rclcpp_action/package.xml

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

rclcpp_action/src/client.cpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <tuple>
 #include <utility>
+#include <variant>
 
 #include "rcl_action/action_client.h"
 #include "rcl_action/wait.h"
@@ -31,6 +32,67 @@
 namespace rclcpp_action
 {
 
+struct ClientBaseData
+{
+  struct FeedbackReadyData
+  {
+    FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), feedback_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> feedback_message;
+  };
+  struct StatusReadyData
+  {
+    StatusReadyData(rcl_ret_t retIn, std::shared_ptr<void> msg)
+    : ret(retIn), status_message(msg) {}
+    rcl_ret_t ret;
+    std::shared_ptr<void> status_message;
+  };
+  struct GoalResponseData
+  {
+    GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), goal_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response;
+  };
+  struct CancelResponseData
+  {
+    CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), cancel_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response;
+  };
+  struct ResultResponseData
+  {
+    ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr<void> response)
+    : ret(retIn), response_header(header), result_response(response) {}
+    rcl_ret_t ret;
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response;
+  };
+
+  std::variant<
+    FeedbackReadyData,
+    StatusReadyData,
+    GoalResponseData,
+    CancelResponseData,
+    ResultResponseData
+  > data;
+
+  explicit ClientBaseData(FeedbackReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(StatusReadyData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(GoalResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(CancelResponseData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ClientBaseData(ResultResponseData && data_in)
+  : data(std::move(data_in)) {}
+};
+
 class ClientBaseImpl
 {
 public:
@@ -94,11 +156,13 @@ public:
   size_t num_clients{0u};
   size_t num_services{0u};
 
-  bool is_feedback_ready{false};
-  bool is_status_ready{false};
-  bool is_goal_response_ready{false};
-  bool is_cancel_response_ready{false};
-  bool is_result_response_ready{false};
+  // Lock for action_client_
+  std::recursive_mutex action_client_mutex_;
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
@@ -119,7 +183,7 @@ public:
   std::mutex cancel_requests_mutex;
 
   std::independent_bits_engine<
-    std::default_random_engine, 8, unsigned int> random_bytes_generator;
+    std::mt19937, 8, unsigned int> random_bytes_generator;
 };
 
 ClientBase::ClientBase(
@@ -143,6 +207,7 @@ bool
 ClientBase::action_server_is_ready() const
 {
   bool is_ready;
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_server_is_available(
     this->pimpl_->node_handle.get(),
     this->pimpl_->client_handle.get(),
@@ -256,6 +321,7 @@ ClientBase::get_number_of_ready_services()
 void
 ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_client(
     wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
   if (RCL_RET_OK != ret) {
@@ -266,23 +332,56 @@ ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 bool
 ClientBase::is_ready(rcl_wait_set_t * wait_set)
 {
-  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
-    wait_set, pimpl_->client_handle.get(),
-    &pimpl_->is_feedback_ready,
-    &pimpl_->is_status_ready,
-    &pimpl_->is_goal_response_ready,
-    &pimpl_->is_cancel_response_ready,
-    &pimpl_->is_result_response_ready);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(
-      ret, "failed to check for any ready entities");
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+    ret = rcl_action_client_wait_set_get_entities_ready(
+      wait_set, pimpl_->client_handle.get(),
+      &is_feedback_ready,
+      &is_status_ready,
+      &is_goal_response_ready,
+      &is_cancel_response_ready,
+      &is_result_response_ready);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "failed to check for any ready entities");
+    }
   }
-  return
-    pimpl_->is_feedback_ready ||
-    pimpl_->is_status_ready ||
-    pimpl_->is_goal_response_ready ||
-    pimpl_->is_cancel_response_ready ||
-    pimpl_->is_result_response_ready;
+
+  pimpl_->next_ready_event = ClientBaseImpl::NO_EVENT_READY;
+
+  if (is_feedback_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::FeedbackSubscription);
+    return true;
+  }
+
+  if (is_status_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::StatusSubscription);
+    return true;
+  }
+
+  if (is_goal_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::GoalClient);
+    return true;
+  }
+
+  if (is_result_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::ResultClient);
+    return true;
+  }
+
+  if (is_cancel_response_ready) {
+    pimpl_->next_ready_event = static_cast<size_t>(EntityType::CancelClient);
+    return true;
+  }
+
+  return false;
 }
 
 void
@@ -432,7 +531,6 @@ ClientBase::set_callback_to_entity(
       }
     };
 
-
   // Set it temporarily to the new callback, while we replace the old one.
   // This two-step setting, prevents a gap where the old std::function has
   // been replaced but the middleware hasn't been told about the new one yet.
@@ -550,140 +648,155 @@ ClientBase::clear_on_ready_callback()
 std::shared_ptr<void>
 ClientBase::take_data()
 {
-  if (pimpl_->is_feedback_ready) {
-    std::shared_ptr<void> feedback_message = this->create_feedback_message();
-    rcl_ret_t ret = rcl_action_take_feedback(
-      pimpl_->client_handle.get(), feedback_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, feedback_message));
-  } else if (pimpl_->is_status_ready) {
-    std::shared_ptr<void> status_message = this->create_status_message();
-    rcl_ret_t ret = rcl_action_take_status(
-      pimpl_->client_handle.get(), status_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, status_message));
-  } else if (pimpl_->is_goal_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> goal_response = this->create_goal_response();
-    rcl_ret_t ret = rcl_action_take_goal_response(
-      pimpl_->client_handle.get(), &response_header, goal_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, goal_response));
-  } else if (pimpl_->is_result_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> result_response = this->create_result_response();
-    rcl_ret_t ret = rcl_action_take_result_response(
-      pimpl_->client_handle.get(), &response_header, result_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, result_response));
-  } else if (pimpl_->is_cancel_response_ready) {
-    rmw_request_id_t response_header;
-    std::shared_ptr<void> cancel_response = this->create_cancel_response();
-    rcl_ret_t ret = rcl_action_take_cancel_response(
-      pimpl_->client_handle.get(), &response_header, cancel_response.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret, response_header, cancel_response));
-  } else {
-    throw std::runtime_error("Taking data from action client but nothing is ready");
+  // next_ready_event is an atomic, caching localy
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ClientBaseImpl::NO_EVENT_READY);
+
+  if (next_ready_event == ClientBaseImpl::NO_EVENT_READY) {
+    throw std::runtime_error("Taking data from action client but no ready event");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ClientBase::take_data_by_entity_id(size_t id)
 {
+  std::shared_ptr<ClientBaseData> data_ptr;
+  rcl_ret_t ret;
+
   // Mark as ready the entity from which we want to take data
   switch (static_cast<EntityType>(id)) {
     case EntityType::GoalClient:
-      pimpl_->is_goal_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> goal_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+
+          goal_response = this->create_goal_response();
+          ret = rcl_action_take_goal_response(
+            pimpl_->client_handle.get(), &response_header, goal_response.get());
+        }
+        data_ptr = std::make_shared<ClientBaseData>(
+          ClientBaseData::GoalResponseData(
+            ret, response_header, goal_response));
+      }
       break;
     case EntityType::ResultClient:
-      pimpl_->is_result_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> result_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          result_response = this->create_result_response();
+          ret = rcl_action_take_result_response(
+            pimpl_->client_handle.get(), &response_header, result_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::ResultResponseData(
+            ret, response_header, result_response));
+      }
       break;
     case EntityType::CancelClient:
-      pimpl_->is_cancel_response_ready = true;
+      {
+        rmw_request_id_t response_header;
+        std::shared_ptr<void> cancel_response;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          cancel_response = this->create_cancel_response();
+          ret = rcl_action_take_cancel_response(
+            pimpl_->client_handle.get(), &response_header, cancel_response.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::CancelResponseData(
+            ret, response_header, cancel_response));
+      }
       break;
     case EntityType::FeedbackSubscription:
-      pimpl_->is_feedback_ready = true;
+      {
+        std::shared_ptr<void> feedback_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          feedback_message = this->create_feedback_message();
+          ret = rcl_action_take_feedback(
+            pimpl_->client_handle.get(), feedback_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::FeedbackReadyData(
+            ret, feedback_message));
+      }
       break;
     case EntityType::StatusSubscription:
-      pimpl_->is_status_ready = true;
+      {
+        std::shared_ptr<void> status_message;
+        {
+          std::lock_guard<std::recursive_mutex> lock(pimpl_->action_client_mutex_);
+          status_message = this->create_status_message();
+          ret = rcl_action_take_status(
+            pimpl_->client_handle.get(), status_message.get());
+        }
+        data_ptr =
+          std::make_shared<ClientBaseData>(
+          ClientBaseData::StatusReadyData(
+            ret, status_message));
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ClientBase::execute(std::shared_ptr<void> & data)
+ClientBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    throw std::runtime_error("Executing action client but 'data' is empty");
   }
 
-  if (pimpl_->is_feedback_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_feedback_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto feedback_message = std::get<1>(*shared_ptr);
-      this->handle_feedback_message(feedback_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
-    }
-  } else if (pimpl_->is_status_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_status_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto status_message = std::get<1>(*shared_ptr);
-      this->handle_status_message(status_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
-    }
-  } else if (pimpl_->is_goal_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_goal_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto goal_response = std::get<2>(*shared_ptr);
-      this->handle_goal_response(response_header, goal_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
-    }
-  } else if (pimpl_->is_result_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_result_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto result_response = std::get<2>(*shared_ptr);
-      this->handle_result_response(response_header, result_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
-    }
-  } else if (pimpl_->is_cancel_response_ready) {
-    auto shared_ptr = std::static_pointer_cast<
-      std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_cancel_response_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto response_header = std::get<1>(*shared_ptr);
-      auto cancel_response = std::get<2>(*shared_ptr);
-      this->handle_cancel_response(response_header, cancel_response);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
-    }
-  } else {
-    throw std::runtime_error("Executing action client but nothing is ready");
-  }
+  std::shared_ptr<ClientBaseData> data_ptr = std::static_pointer_cast<ClientBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ClientBaseData::FeedbackReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_feedback_message(data.feedback_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::StatusReadyData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_status_message(data.status_message);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::GoalResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_goal_response(data.response_header, data.goal_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::ResultResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_result_response(data.response_header, data.result_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response");
+        }
+      }
+      if constexpr (std::is_same_v<T, ClientBaseData::CancelResponseData>) {
+        if (RCL_RET_OK == data.ret) {
+          this->handle_cancel_response(data.response_header, data.cancel_response);
+        } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) {
+          rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response");
+        }
+      }
+    }, data_ptr->data);
 }
 
 }  // namespace rclcpp_action

rclcpp_action/src/server.cpp

@@ -18,6 +18,7 @@
 #include <tuple>
 #include <unordered_map>
 #include <utility>
+#include <variant>
 #include <vector>
 
 #include "rcl_action/action_server.h"
@@ -33,8 +34,50 @@
 using rclcpp_action::ServerBase;
 using rclcpp_action::GoalUUID;
 
+struct ServerBaseData;
+
 namespace rclcpp_action
 {
+
+struct ServerBaseData
+{
+  using GoalRequestData = std::tuple<
+    rcl_ret_t,
+    const rcl_action_goal_info_t,
+    rmw_request_id_t,
+    std::shared_ptr<void>
+  >;
+
+  using CancelRequestData = std::tuple<
+    rcl_ret_t,
+    std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
+    rmw_request_id_t
+  >;
+
+  using ResultRequestData = std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>;
+
+  using GoalExpiredData = struct Empty {};
+
+  std::variant<GoalRequestData, CancelRequestData, ResultRequestData, GoalExpiredData> data;
+
+  explicit ServerBaseData(GoalRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(CancelRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(ResultRequestData && data_in)
+  : data(std::move(data_in)) {}
+  explicit ServerBaseData(GoalExpiredData && data_in)
+  : data(std::move(data_in)) {}
+};
+
+enum class ActionEventType : std::size_t
+{
+  GoalService,
+  ResultService,
+  CancelService,
+  Expired,
+};
+
 class ServerBaseImpl
 {
 public:
@@ -60,11 +103,6 @@ public:
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  std::atomic<bool> goal_request_ready_{false};
-  std::atomic<bool> cancel_request_ready_{false};
-  std::atomic<bool> result_request_ready_{false};
-  std::atomic<bool> goal_expired_{false};
-
   // Lock for unordered_maps
   std::recursive_mutex unordered_map_mutex_;
 
@@ -75,8 +113,15 @@ public:
   // rcl goal handles are kept so api to send result doesn't try to access freed memory
   std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
 
+
+  // next ready event for taking, will be set by is_ready and will be processed by take_data
+  std::atomic<size_t> next_ready_event;
+  // used to indicate that next_ready_event has no ready event for processing
+  static constexpr size_t NO_EVENT_READY = std::numeric_limits<size_t>::max();
+
   rclcpp::Logger logger_;
 };
+
 }  // namespace rclcpp_action
 
 ServerBase::ServerBase(
@@ -195,124 +240,166 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set)
       &goal_expired);
   }
 
-  pimpl_->goal_request_ready_ = goal_request_ready;
-  pimpl_->cancel_request_ready_ = cancel_request_ready;
-  pimpl_->result_request_ready_ = result_request_ready;
-  pimpl_->goal_expired_ = goal_expired;
-
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_.load() ||
-         pimpl_->cancel_request_ready_.load() ||
-         pimpl_->result_request_ready_.load() ||
-         pimpl_->goal_expired_.load();
+  pimpl_->next_ready_event = ServerBaseImpl::NO_EVENT_READY;
+
+  if (goal_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::GoalService);
+    return true;
+  }
+
+  if (cancel_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::CancelService);
+    return true;
+  }
+
+  if (result_request_ready) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::ResultService);
+    return true;
+  }
+
+  if (goal_expired) {
+    pimpl_->next_ready_event = static_cast<uint32_t>(ActionEventType::Expired);
+    return true;
+  }
+
+  return false;
 }
 
 std::shared_ptr<void>
 ServerBase::take_data()
 {
-  if (pimpl_->goal_request_ready_.load()) {
-    rcl_ret_t ret;
-    rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
-    rmw_request_id_t request_header;
+  size_t next_ready_event = pimpl_->next_ready_event.exchange(ServerBaseImpl::NO_EVENT_READY);
 
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-
-    std::shared_ptr<void> message = create_goal_request();
-    ret = rcl_action_take_goal_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      message.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(
-        ret,
-        goal_info,
-        request_header, message));
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    rcl_ret_t ret;
-    rmw_request_id_t request_header;
-
-    // Initialize cancel request
-    auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
-
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_cancel_request(
-      pimpl_->action_server_.get(),
-      &request_header,
-      request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared
-      <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(ret, request, request_header));
-  } else if (pimpl_->result_request_ready_.load()) {
-    rcl_ret_t ret;
-    // Get the result request message
-    rmw_request_id_t request_header;
-    std::shared_ptr<void> result_request = create_result_request();
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-    ret = rcl_action_take_result_request(
-      pimpl_->action_server_.get(), &request_header, result_request.get());
-
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(
-        ret, result_request, request_header));
-  } else if (pimpl_->goal_expired_.load()) {
-    return nullptr;
-  } else {
-    throw std::runtime_error("Taking data from action server but nothing is ready");
+  if (next_ready_event == ServerBaseImpl::NO_EVENT_READY) {
+    throw std::runtime_error("Taking data from action server but no ready event");
   }
+
+  return take_data_by_entity_id(next_ready_event);
 }
 
 std::shared_ptr<void>
 ServerBase::take_data_by_entity_id(size_t id)
 {
+  static_assert(
+    static_cast<size_t>(EntityType::GoalService) ==
+    static_cast<size_t>(ActionEventType::GoalService));
+  static_assert(
+    static_cast<size_t>(EntityType::ResultService) ==
+    static_cast<size_t>(ActionEventType::ResultService));
+  static_assert(
+    static_cast<size_t>(EntityType::CancelService) ==
+    static_cast<size_t>(ActionEventType::CancelService));
+
+  std::shared_ptr<ServerBaseData> data_ptr;
   // Mark as ready the entity from which we want to take data
-  switch (static_cast<EntityType>(id)) {
-    case EntityType::GoalService:
-      pimpl_->goal_request_ready_ = true;
+  switch (static_cast<ActionEventType>(id)) {
+    case ActionEventType::GoalService:
+      {
+        rcl_ret_t ret;
+        rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+        rmw_request_id_t request_header;
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
+        std::shared_ptr<void> message = create_goal_request();
+        ret = rcl_action_take_goal_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          message.get());
+
+        data_ptr = std::make_shared<ServerBaseData>(
+          ServerBaseData::GoalRequestData(ret, goal_info, request_header, message));
+      }
       break;
-    case EntityType::ResultService:
-      pimpl_->result_request_ready_ = true;
+    case ActionEventType::ResultService:
+      {
+        rcl_ret_t ret;
+        // Get the result request message
+        rmw_request_id_t request_header;
+        std::shared_ptr<void> result_request = create_result_request();
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_result_request(
+          pimpl_->action_server_.get(), &request_header, result_request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::ResultRequestData(ret, result_request, request_header));
+      }
       break;
-    case EntityType::CancelService:
-      pimpl_->cancel_request_ready_ = true;
+    case ActionEventType::CancelService:
+      {
+        rcl_ret_t ret;
+        rmw_request_id_t request_header;
+
+        // Initialize cancel request
+        auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+        std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+        ret = rcl_action_take_cancel_request(
+          pimpl_->action_server_.get(),
+          &request_header,
+          request.get());
+
+        data_ptr =
+          std::make_shared<ServerBaseData>(
+          ServerBaseData::CancelRequestData(ret, request, request_header));
+      }
+      break;
+    case ActionEventType::Expired:
+      {
+        data_ptr =
+          std::make_shared<ServerBaseData>(ServerBaseData::GoalExpiredData());
+      }
       break;
   }
 
-  return take_data();
+  return std::static_pointer_cast<void>(data_ptr);
 }
 
 void
-ServerBase::execute(std::shared_ptr<void> & data)
+ServerBase::execute(std::shared_ptr<void> & data_in)
 {
-  if (!data && !pimpl_->goal_expired_.load()) {
-    throw std::runtime_error("'data' is empty");
+  if (!data_in) {
+    throw std::runtime_error("Executing action server but 'data' is empty");
   }
 
-  if (pimpl_->goal_request_ready_.load()) {
-    execute_goal_request_received(data);
-  } else if (pimpl_->cancel_request_ready_.load()) {
-    execute_cancel_request_received(data);
-  } else if (pimpl_->result_request_ready_.load()) {
-    execute_result_request_received(data);
-  } else if (pimpl_->goal_expired_.load()) {
-    execute_check_expired_goals();
-  } else {
-    throw std::runtime_error("Executing action server but nothing is ready");
-  }
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data_in);
+
+  std::visit(
+    [&](auto && data) -> void {
+      using T = std::decay_t<decltype(data)>;
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalRequestData>) {
+        execute_goal_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::CancelRequestData>) {
+        execute_cancel_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::ResultRequestData>) {
+        execute_result_request_received(data_in);
+      }
+      if constexpr (std::is_same_v<T, ServerBaseData::GoalExpiredData>) {
+        execute_check_expired_goals();
+      }
+    },
+    data_ptr->data);
 }
 
 void
 ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, rcl_action_goal_info_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
-  rcl_ret_t ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::GoalRequestData & gData(
+    std::get<ServerBaseData::GoalRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  rcl_action_goal_info_t goal_info = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+  const std::shared_ptr<void> message = std::get<3>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -321,14 +408,6 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr);
-  rmw_request_id_t request_header = std::get<2>(*shared_ptr);
-  std::shared_ptr<void> message = std::get<3>(*shared_ptr);
-
-  bool expected = true;
-  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
-    return;
-  }
 
   GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
@@ -345,7 +424,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   }
 
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send goal response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
   }
 
   const auto status = response_pair.first;
@@ -403,10 +491,15 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
-      rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::CancelRequestData & gData(
+    std::get<ServerBaseData::CancelRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<action_msgs::srv::CancelGoal::Request> request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -415,9 +508,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
-  pimpl_->cancel_request_ready_ = false;
 
   // Convert c++ message to C message
   rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
@@ -484,6 +574,15 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
       pimpl_->action_server_.get(), &request_header, response.get());
   }
 
+  if (ret == RCL_RET_TIMEOUT) {
+    GoalUUID uuid = request->goal_info.goal_id.uuid;
+    RCLCPP_WARN(
+      pimpl_->logger_,
+      "Failed to send cancel response %s (timeout): %s",
+      to_string(uuid).c_str(), rcl_get_error_string().str);
+    rcl_reset_error();
+    return;
+  }
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -493,9 +592,14 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
 void
 ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
 {
-  auto shared_ptr = std::static_pointer_cast
-    <std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
-  auto ret = std::get<0>(*shared_ptr);
+  std::shared_ptr<ServerBaseData> data_ptr = std::static_pointer_cast<ServerBaseData>(data);
+  const ServerBaseData::ResultRequestData & gData(
+    std::get<ServerBaseData::ResultRequestData>(data_ptr->data));
+
+  rcl_ret_t ret = std::get<0>(gData);
+  std::shared_ptr<void> result_request = std::get<1>(gData);
+  rmw_request_id_t request_header = std::get<2>(gData);
+
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -504,10 +608,7 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
-  auto result_request = std::get<1>(*shared_ptr);
-  auto request_header = std::get<2>(*shared_ptr);
 
-  pimpl_->result_request_ready_ = false;
   std::shared_ptr<void> result_response;
 
   // check if the goal exists
@@ -539,6 +640,14 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     rcl_ret_t rcl_ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (rcl_ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send result response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (RCL_RET_OK != rcl_ret) {
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
@@ -672,7 +781,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
       for (auto & request_header : iter->second) {
         rcl_ret_t ret = rcl_action_send_result_response(
           pimpl_->action_server_.get(), &request_header, result_msg.get());
-        if (RCL_RET_OK != ret) {
+        if (ret == RCL_RET_TIMEOUT) {
+          RCLCPP_WARN(
+            pimpl_->logger_,
+            "Failed to send result response %s (timeout): %s",
+            to_string(uuid).c_str(), rcl_get_error_string().str);
+          rcl_reset_error();
+        } else if (RCL_RET_OK != ret) {
           rclcpp::exceptions::throw_from_rcl_error(ret);
         }
       }

rclcpp_action/src/types.cpp

@@ -33,6 +33,9 @@ to_string(const GoalUUID & goal_id)
 void
 convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
 {
+  if (info == nullptr) {
+    throw std::invalid_argument("info is nullptr");
+  }
   for (size_t i = 0; i < 16; ++i) {
     info->goal_id.uuid[i] = goal_id[i];
   }
@@ -41,6 +44,9 @@ convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
 void
 convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id)
 {
+  if (goal_id == nullptr) {
+    throw std::invalid_argument("goal_id is nullptr");
+  }
   for (size_t i = 0; i < 16; ++i) {
     (*goal_id)[i] = info.goal_id.uuid[i];
   }

rclcpp_action/test/test_client.cpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
 #include <future>
 #include <map>
 #include <memory>
 #include <string>
 #include <utility>
 #include <thread>
-#include <chrono>
 
 #include "gtest/gtest.h"
 

rclcpp_action/test/test_types.cpp

@@ -40,6 +40,7 @@ TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) {
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
     goal_id[i] = i;
   }
+  ASSERT_THROW(rclcpp_action::convert(goal_id, nullptr), std::invalid_argument);
   rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
   rclcpp_action::convert(goal_id, &goal_info);
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {
@@ -53,6 +54,7 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) {
     goal_info.goal_id.uuid[i] = i;
   }
 
+  ASSERT_THROW(rclcpp_action::convert(goal_info, nullptr), std::invalid_argument);
   rclcpp_action::GoalUUID goal_id;
   rclcpp_action::convert(goal_id, &goal_info);
   for (uint8_t i = 0; i < UUID_SIZE; ++i) {

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,48 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+16.0.18 (2026-02-09)
+--------------------
+
+16.0.17 (2025-12-23)
+--------------------
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+
+16.0.14 (2025-07-16)
+--------------------
+
+16.0.13 (2025-06-23)
+--------------------
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* Contributors: mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+* Redundant .c_str() usage in rclcpp_components triggers ament_clang_tidy warning (`#2718 <https://github.com/ros2/rclcpp/issues/2718>`_)
+* Contributors: LihanChen2004
+
+16.0.11 (2024-11-25)
+--------------------
+
+16.0.10 (2024-07-26)
+--------------------
+
+16.0.9 (2024-05-15)
+-------------------
+
+16.0.8 (2024-01-24)
+-------------------
+* Add missing header required by the rclcpp::NodeOptions type (`#2324 <https://github.com/ros2/rclcpp/issues/2324>`_) (`#2407 <https://github.com/ros2/rclcpp/issues/2407>`_)
+* fix(rclcpp_components): increase the service queue sizes in component_container (backport `#2363 <https://github.com/ros2/rclcpp/issues/2363>`_) (`#2380 <https://github.com/ros2/rclcpp/issues/2380>`_)
+* Contributors: mergify[bot]
+
 16.0.7 (2023-11-13)
 -------------------
 

rclcpp_components/QUALITY_DECLARATION.md

@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 # rclcpp_components Quality Declaration
 
 The package `rclcpp_components` claims to be in the **Quality Level 1** category.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
@@ -53,7 +53,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d
 
 ### Continuous Integration [2.iv]
 
-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
 
 Currently nightly results can be seen here:
 
@@ -191,7 +191,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github
 
 ## Platform Support [6]
 
-`rclcpp_components` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp_components` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
 
 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_components/)
@@ -203,4 +203,4 @@ Currently nightly build status can be seen here:
 
 ### Vulnerability Disclosure Policy [7.i]
 
-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp_components/cmake/rclcpp_components_package_hook.cmake

@@ -13,9 +13,20 @@
 # limitations under the License.
 
 # register node plugins
-list(REMOVE_DUPLICATES _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES)
-foreach(resource_index ${_RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES})
+# The internal data is stored in a project directory scoped properties to allow
+# registering the components from nested scopes in CMake, where variables
+# would not propagate out.
+get_property(_rclcpp_components_package_resource_indices
+  DIRECTORY "${PROJECT_SOURCE_DIR}"
+  PROPERTY _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES
+)
+list(REMOVE_DUPLICATES _rclcpp_components_package_resource_indices)
+foreach(resource_index ${_rclcpp_components_package_resource_indices})
+  get_property(_rclcpp_components_nodes
+    DIRECTORY "${PROJECT_SOURCE_DIR}"
+    PROPERTY "_RCLCPP_COMPONENTS_${resource_index}__NODES"
+  )
   ament_index_register_resource(
-    ${resource_index} CONTENT "${_RCLCPP_COMPONENTS_${resource_index}__NODES}")
+    ${resource_index} CONTENT "${_rclcpp_components_nodes}")
 endforeach()
 

rclcpp_components/cmake/rclcpp_components_register_node.cmake

@@ -55,15 +55,19 @@ macro(rclcpp_components_register_node target)
 
   set(component ${ARGS_PLUGIN})
   set(node ${ARGS_EXECUTABLE})
-  _rclcpp_components_register_package_hook()
   set(_path "lib")
   set(library_name "$<TARGET_FILE_NAME:${target}>")
   if(WIN32)
     set(_path "bin")
   endif()
-  set(_RCLCPP_COMPONENTS_${resource_index}__NODES
-    "${_RCLCPP_COMPONENTS_${resource_index}__NODES}${component};${_path}/$<TARGET_FILE_NAME:${target}>\n")
-  list(APPEND _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES ${resource_index})
+  set_property(
+    DIRECTORY "${PROJECT_SOURCE_DIR}"
+    APPEND_STRING PROPERTY _RCLCPP_COMPONENTS_${resource_index}__NODES
+    "${component};${_path}/$<TARGET_FILE_NAME:${target}>\n")
+  set_property(
+    DIRECTORY "${PROJECT_SOURCE_DIR}"
+    APPEND PROPERTY _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES
+    ${resource_index})
 
   configure_file(${rclcpp_components_NODE_TEMPLATE}
     ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in)

rclcpp_components/cmake/rclcpp_components_register_nodes.cmake

@@ -47,7 +47,6 @@ macro(rclcpp_components_register_nodes target)
   endif()
 
   if(${ARGC} GREATER 0)
-    _rclcpp_components_register_package_hook()
     set(_unique_names)
     foreach(_arg ${ARGS_UNPARSED_ARGUMENTS})
       if(_arg IN_LIST _unique_names)
@@ -63,9 +62,14 @@ macro(rclcpp_components_register_nodes target)
       else()
         set(_path "lib")
       endif()
-      set(_RCLCPP_COMPONENTS_${resource_index}__NODES
-        "${_RCLCPP_COMPONENTS_${resource_index}__NODES}${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
-      list(APPEND _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES ${resource_index})
+      set_property(
+        DIRECTORY "${PROJECT_SOURCE_DIR}"
+        APPEND_STRING PROPERTY _RCLCPP_COMPONENTS_${resource_index}__NODES
+        "${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
+      set_property(
+        DIRECTORY "${PROJECT_SOURCE_DIR}"
+        APPEND PROPERTY _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES
+        ${resource_index})
     endforeach()
   endif()
 endmacro()

rclcpp_components/include/rclcpp_components/node_factory.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 #define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
 
+#include "rclcpp/node_options.hpp"
 #include "rclcpp_components/node_instance_wrapper.hpp"
 
 namespace rclcpp_components

rclcpp_components/package.xml

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

rclcpp_components/rclcpp_components-extras.cmake.in

@@ -25,6 +25,8 @@ macro(_rclcpp_components_register_package_hook)
   endif()
 endmacro()
 
+_rclcpp_components_register_package_hook()
+
 get_filename_component(@PROJECT_NAME@_SHARE_DIR "${@PROJECT_NAME@_DIR}" DIRECTORY)
 set(@PROJECT_NAME@_NODE_TEMPLATE "${@PROJECT_NAME@_SHARE_DIR}/node_main.cpp.in")
 

rclcpp_components/src/component_container_mt.cpp

@@ -23,16 +23,16 @@ int main(int argc, char * argv[])
   /// Component container with a multi-threaded executor.
   rclcpp::init(argc, argv);
 
-  auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
-  auto node = std::make_shared<rclcpp_components::ComponentManager>();
+  rclcpp::executors::MultiThreadedExecutor::SharedPtr exec = nullptr;
+  const auto node = std::make_shared<rclcpp_components::ComponentManager>();
   if (node->has_parameter("thread_num")) {
     const auto thread_num = node->get_parameter("thread_num").as_int();
     exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(
       rclcpp::ExecutorOptions{}, thread_num);
-    node->set_executor(exec);
   } else {
-    node->set_executor(exec);
+    exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   }
+  node->set_executor(exec);
   exec->add_node(node);
   exec->spin();
 }

rclcpp_components/src/component_manager.cpp

@@ -37,12 +37,16 @@ ComponentManager::ComponentManager(
 : Node(std::move(node_name), node_options),
   executor_(executor)
 {
+  rmw_qos_profile_t service_qos = rmw_qos_profile_services_default;
+  service_qos.depth = 200;
   loadNode_srv_ = create_service<LoadNode>(
     "~/_container/load_node",
-    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_load_node, this, _1, _2, _3),
+    service_qos);
   unloadNode_srv_ = create_service<UnloadNode>(
     "~/_container/unload_node",
-    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3));
+    std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3),
+    service_qos);
   listNodes_srv_ = create_service<ListNodes>(
     "~/_container/list_nodes",
     std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3));

rclcpp_components/src/node_main.cpp.in

@@ -40,8 +40,7 @@ int main(int argc, char * argv[])
   auto loader = new class_loader::ClassLoader(library_name);
   auto classes = loader->getAvailableClasses<rclcpp_components::NodeFactory>();
   for (const auto & clazz : classes) {
-    std::string name = clazz.c_str();
-    if (name.compare(class_name) == 0) {
+    if (clazz.compare(class_name) == 0) {
       RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str());
       std::shared_ptr<rclcpp_components::NodeFactory> node_factory = nullptr;
       try {

rclcpp_components/test/test_component_manager_api.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
 #include <string>
 

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,55 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+16.0.18 (2026-02-09)
+--------------------
+
+16.0.17 (2025-12-23)
+--------------------
+* [Humble] Implement Unified Node Interface (NodeInterfaces class) (backport `#2041 <https://github.com/ros2/rclcpp/issues/2041>`_) (`#3002 <https://github.com/ros2/rclcpp/issues/3002>`_)
+* Contributors: fabianhirmann
+
+16.0.16 (2025-11-18)
+--------------------
+* Fix REP url locations (backport `#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2991 <https://github.com/ros2/rclcpp/issues/2991>`_)
+* Add get_parameter_or overload returning value or alternative (`#2973 <https://github.com/ros2/rclcpp/issues/2973>`_) (`#2978 <https://github.com/ros2/rclcpp/issues/2978>`_)
+* Contributors: mergify[bot]
+
+16.0.15 (2025-09-11)
+--------------------
+* Add a clearer warning message, the old one lacked information and was misleading (`#2924 <https://github.com/ros2/rclcpp/issues/2924>`_)
+* Contributors: Peter Mitrano (AR)
+
+16.0.14 (2025-07-16)
+--------------------
+
+16.0.13 (2025-06-23)
+--------------------
+* Added missing chrono includes (backport `#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2857 <https://github.com/ros2/rclcpp/issues/2857>`_)
+* should pull valid transition before trying to change the state. (backport `#2774 <https://github.com/ros2/rclcpp/issues/2774>`_) (`#2785 <https://github.com/ros2/rclcpp/issues/2785>`_)
+* Contributors: mergify[bot]
+
+16.0.12 (2025-03-25)
+--------------------
+
+16.0.11 (2024-11-25)
+--------------------
+
+16.0.10 (2024-07-26)
+--------------------
+* revert call shutdown in LifecycleNode destructor (Humble) (`#2560 <https://github.com/ros2/rclcpp/issues/2560>`_)
+* lifecycle node dtor shutdown should be called only in primary state. (`#2544 <https://github.com/ros2/rclcpp/issues/2544>`_)
+* rclcpp::shutdown should not be called before LifecycleNode dtor. (backport `#2527 <https://github.com/ros2/rclcpp/issues/2527>`_) (`#2538 <https://github.com/ros2/rclcpp/issues/2538>`_)
+* Contributors: Tomoya Fujita, mergify[bot]
+
+16.0.9 (2024-05-15)
+-------------------
+* call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 <https://github.com/ros2/rclcpp/issues/2450>`_) (`#2491 <https://github.com/ros2/rclcpp/issues/2491>`_)
+* Contributors: mergify[bot]
+
+16.0.8 (2024-01-24)
+-------------------
+
 16.0.7 (2023-11-13)
 -------------------
 

rclcpp_lifecycle/QUALITY_DECLARATION.md

@@ -1,10 +1,10 @@
-This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html).
+This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 # rclcpp_lifecycle Quality Declaration
 
 The package `rclcpp_lifecycle` claims to be in the **Quality Level 1** category when used with a **Quality Level 1** middleware.
 
-Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://reps.openrobotics.org/rep-2004/) of the ROS2 developer guide.
 
 ## Version Policy [1]
 
@@ -53,7 +53,7 @@ All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://d
 
 ### Continuous Integration [2.iv]
 
-All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers)
+All pull requests must pass CI on all [tier 1 platforms](https://reps.openrobotics.org/rep-2000/#support-tiers)
 
 Currently nightly results can be seen here:
 
@@ -191,7 +191,7 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github
 
 ## Platform Support [6]
 
-`rclcpp_lifecycle` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them.
+`rclcpp_lifecycle` supports all of the tier 1 platforms as described in [REP-2000](https://reps.openrobotics.org/rep-2000/#support-tiers), and tests each change against all of them.
 
 Currently nightly build status can be seen here:
 * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_lifecycle/)
@@ -203,4 +203,4 @@ Currently nightly build status can be seen here:
 
 ### Vulnerability Disclosure Policy [7.i]
 
-This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html).
+This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://reps.openrobotics.org/rep-2006/).

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -458,6 +458,16 @@ public:
     ParameterT & value,
     const ParameterT & alternative_value) const;
 
+  /// Return the parameter value, or the "alternative_value" if not set.
+  /**
+   * \sa rclcpp::Node::get_parameter_or
+   */
+  template<typename ParameterT>
+  ParameterT
+  get_parameter_or(
+    const std::string & name,
+    const ParameterT & alternative_value) const;
+
   /// Return the parameters by the given parameter names.
   /**
    * \sa rclcpp::Node::get_parameters

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -294,5 +294,16 @@ LifecycleNode::get_parameter_or(
   return got_parameter;
 }
 
+template<typename ParameterT>
+ParameterT
+LifecycleNode::get_parameter_or(
+  const std::string & name,
+  const ParameterT & alternative_value) const
+{
+  ParameterT parameter;
+  get_parameter_or(name, parameter, alternative_value);
+  return parameter;
+}
+
 }  // namespace rclcpp_lifecycle
 #endif  // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_

rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 
 namespace rclcpp_lifecycle
 {
@@ -108,4 +109,8 @@ public:
 
 }  // namespace node_interfaces
 }  // namespace rclcpp_lifecycle
+
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node)
+
 #endif  // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_

rclcpp_lifecycle/package.xml

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

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -401,6 +401,7 @@ public:
     constexpr bool publish_update = true;
     State initial_state;
     unsigned int current_state_id;
+    rcl_lifecycle_transition_t const * original_transition{nullptr};
 
     {
       std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
@@ -414,6 +415,9 @@ public:
       // keep the initial state to pass to a transition callback
       initial_state = State(state_machine_.current_state);
 
+      original_transition =
+        rcl_lifecycle_get_transition_by_id(state_machine_.current_state, transition_id);
+
       if (
         rcl_lifecycle_trigger_transition_by_id(
           &state_machine_, transition_id, publish_update) != RCL_RET_OK)
@@ -458,12 +462,14 @@ public:
       current_state_id = state_machine_.current_state->id;
     }
 
-    // error handling ?!
+    // error handling
     // TODO(karsten1987): iterate over possible ret value
     if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
-      RCLCPP_WARN(
-        node_logging_interface_->get_logger(),
-        "Error occurred while doing error handling.");
+      if (original_transition) {
+        RCLCPP_WARN(
+          node_logging_interface_->get_logger(),
+          "Callback returned ERROR during the transition: %s", original_transition->label);
+      }
 
       auto error_cb_code = execute_callback(current_state_id, initial_state);
       auto error_cb_label = get_label_for_return_code(error_cb_code);
@@ -535,15 +541,22 @@ public:
   trigger_transition(uint8_t transition_id)
   {
     LifecycleNodeInterface::CallbackReturn error;
-    change_state(transition_id, error);
-    (void) error;
-    return get_current_state();
+    return trigger_transition(transition_id, error);
   }
 
   const State &
   trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code)
   {
-    change_state(transition_id, cb_return_code);
+    const rcl_lifecycle_transition_t * transition;
+    {
+      std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+
+      transition =
+        rcl_lifecycle_get_transition_by_id(state_machine_.current_state, transition_id);
+    }
+    if (transition) {
+      change_state(static_cast<uint8_t>(transition->id), cb_return_code);
+    }
     return get_current_state();
   }
 

rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp

@@ -14,6 +14,7 @@
 
 #include <benchmark/benchmark.h>
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -14,6 +14,7 @@
 
 
 #include <gtest/gtest.h>
+#include <chrono>
 #include <map>
 #include <memory>
 #include <set>
@@ -264,11 +265,146 @@ TEST_F(TestDefaultStateMachine, trigger_transition) {
   ASSERT_EQ(
     State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
       rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
+  // supposed to fail because primary state is NOT active
+  ASSERT_EQ(
+    State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+      rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVE_SHUTDOWN)).id());
+  // supposed to fail because primary state is NOT inactive
+  ASSERT_EQ(
+    State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+      rclcpp_lifecycle::Transition(Transition::TRANSITION_INACTIVE_SHUTDOWN)).id());
   ASSERT_EQ(
     State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
       rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
 }
 
+TEST_F(TestDefaultStateMachine, trigger_transition_shutdown_id) {
+  // test Transition::TRANSITION_ACTIVE_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVE_SHUTDOWN)).id());
+  }
+
+  // test Transition::TRANSITION_INACTIVE_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
+    // supposed to fail because primary state is NOT active
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVE_SHUTDOWN)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_INACTIVE_SHUTDOWN)).id());
+  }
+
+  // test Transition::TRANSITION_UNCONFIGURED_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
+    // supposed to fail because primary state is NOT active
+    ASSERT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVE_SHUTDOWN)).id());
+    // supposed to fail because primary state is NOT inactive
+    ASSERT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_INACTIVE_SHUTDOWN)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id());
+  }
+}
+
+TEST_F(TestDefaultStateMachine, trigger_transition_shutdown_label) {
+  // test Transition::TRANSITION_ACTIVE_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->shutdown().id());
+  }
+
+  // test Transition::TRANSITION_INACTIVE_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->shutdown().id());
+  }
+
+  // test Transition::TRANSITION_UNCONFIGURED_SHUTDOWN
+  {
+    auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+    EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition(
+        rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id());
+    ASSERT_EQ(
+      State::PRIMARY_STATE_FINALIZED, test_node->shutdown().id());
+  }
+}
+
 TEST_F(TestDefaultStateMachine, trigger_transition_rcl_errors) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
 
@@ -635,6 +771,26 @@ TEST_F(TestDefaultStateMachine, check_parameters) {
   EXPECT_TRUE(parameter.as_bool());
 }
 
+TEST_F(TestDefaultStateMachine, test_get_parameter_or) {
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+  const std::string param_name = "test_param";
+  int param_int = -999;
+
+  // Parameter does not exist, should return "or" value
+  EXPECT_FALSE(test_node->get_parameter_or(param_name, param_int, 123));
+  EXPECT_EQ(param_int, 123);
+  EXPECT_EQ(test_node->get_parameter_or(param_name, 456), 456);
+
+  // Declare param_int
+  test_node->declare_parameter(param_name, rclcpp::ParameterValue(789));
+
+  // Parameter exists, should return existing value
+  EXPECT_TRUE(test_node->get_parameter_or(param_name, param_int, 123));
+  EXPECT_EQ(param_int, 789);
+  EXPECT_EQ(test_node->get_parameter_or(param_name, 456), 789);
+}
+
 TEST_F(TestDefaultStateMachine, test_getters) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
   auto options = test_node->get_node_options();

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -48,24 +48,10 @@ public:
   explicit EmptyLifecycleNode(const std::string & node_name)
   : rclcpp_lifecycle::LifecycleNode(node_name)
   {
-    rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
-    publisher_ =
-      std::make_shared<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>>(
-      get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options);
-    add_managed_entity(publisher_);
-
     // For coverage this is being added here
     auto timer = create_wall_timer(std::chrono::seconds(1), []() {});
     add_timer_handle(timer);
   }
-
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher()
-  {
-    return publisher_;
-  }
-
-private:
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher_;
 };
 
 class TestLifecyclePublisher : public ::testing::Test
@@ -74,77 +60,85 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
-    node_ = std::make_shared<EmptyLifecycleNode>("node");
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
-
-protected:
-  std::shared_ptr<EmptyLifecycleNode> node_;
 };
 
 TEST_F(TestLifecyclePublisher, publish_managed_by_node) {
+  auto node = std::make_shared<EmptyLifecycleNode>("node");
+
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher =
+    node->create_publisher<test_msgs::msg::Empty>(std::string("topic"), rclcpp::QoS(10), options);
+
   // transition via LifecycleNode
   auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   auto ret = reset_key;
 
-  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node_->get_current_state().id());
-  node_->trigger_transition(
+  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node->get_current_state().id());
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
-  node_->trigger_transition(
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
-  EXPECT_TRUE(node_->publisher()->is_activated());
+  EXPECT_TRUE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
-  node_->trigger_transition(
+  node->trigger_transition(
     rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret);
   ASSERT_EQ(success, ret);
   ret = reset_key;
-  EXPECT_FALSE(node_->publisher()->is_activated());
+  EXPECT_FALSE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
 }
 
 TEST_F(TestLifecyclePublisher, publish) {
+  auto node = std::make_shared<EmptyLifecycleNode>("node");
+
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>> publisher =
+    node->create_publisher<test_msgs::msg::Empty>(std::string("topic"), rclcpp::QoS(10), options);
+
   // transition via LifecyclePublisher
-  node_->publisher()->on_deactivate();
-  EXPECT_FALSE(node_->publisher()->is_activated());
+  publisher->on_deactivate();
+  EXPECT_FALSE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
-  node_->publisher()->on_activate();
-  EXPECT_TRUE(node_->publisher()->is_activated());
+  publisher->on_activate();
+  EXPECT_TRUE(publisher->is_activated());
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr));
+    EXPECT_NO_THROW(publisher->publish(*msg_ptr));
   }
   {
     auto msg_ptr = std::make_unique<test_msgs::msg::Empty>();
-    EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr)));
+    EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr)));
   }
 }