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

(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,60 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+29.5.7 (2026-02-09)
+-------------------
+* include the 1st spin that might throw the exception. (`#3042 <https://github.com/ros2/rclcpp/issues/3042>`_) (`#3044 <https://github.com/ros2/rclcpp/issues/3044>`_)
+* print warning message on owner node if the parameter operation fails. (`#3037 <https://github.com/ros2/rclcpp/issues/3037>`_) (`#3038 <https://github.com/ros2/rclcpp/issues/3038>`_)
+* remove test_static_executor_entities_collector.cpp (`#3041 <https://github.com/ros2/rclcpp/issues/3041>`_) (`#3045 <https://github.com/ros2/rclcpp/issues/3045>`_)
+* fix context in wait for message wait set (`#3030 <https://github.com/ros2/rclcpp/issues/3030>`_) (`#3031 <https://github.com/ros2/rclcpp/issues/3031>`_)
+* Improve the robustness of the TopicEndpointInfo constructor (`#3013 <https://github.com/ros2/rclcpp/issues/3013>`_) (`#3014 <https://github.com/ros2/rclcpp/issues/3014>`_)
+* Contributors: mergify[bot]
+
+29.5.6 (2025-12-23)
+-------------------
+* Unified Node Interfaces: Add const version of get_node_x_interface() (`#3006 <https://github.com/ros2/rclcpp/issues/3006>`_) (`#3008 <https://github.com/ros2/rclcpp/issues/3008>`_)
+* remove I/O from signal handler. (`#3000 <https://github.com/ros2/rclcpp/issues/3000>`_)
+* correct test function descriptions (`#2970 <https://github.com/ros2/rclcpp/issues/2970>`_) (`#2992 <https://github.com/ros2/rclcpp/issues/2992>`_)
+* Contributors: Tomoya Fujita, mergify[bot]
+
+29.5.5 (2025-11-18)
+-------------------
+* Fix REP url locations (`#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2989 <https://github.com/ros2/rclcpp/issues/2989>`_)
+* Contributors: mergify[bot]
+
+29.5.4 (2025-10-21)
+-------------------
+
+29.5.3 (2025-09-11)
+-------------------
+* Fix: improve exception context for parameter_value_from (`#2917 <https://github.com/ros2/rclcpp/issues/2917>`_) (`#2919 <https://github.com/ros2/rclcpp/issues/2919>`_)
+* Allow for implicitly convertable loggers as well (`#2922 <https://github.com/ros2/rclcpp/issues/2922>`_) (`#2936 <https://github.com/ros2/rclcpp/issues/2936>`_)
+* Fix `start_type_description_service` param handling (`#2897 <https://github.com/ros2/rclcpp/issues/2897>`_) (`#2908 <https://github.com/ros2/rclcpp/issues/2908>`_)
+* Add qos parameter for wait_for_message function (`#2903 <https://github.com/ros2/rclcpp/issues/2903>`_) (`#2905 <https://github.com/ros2/rclcpp/issues/2905>`_)
+* Expose `typesupport_helpers` API needed for the Rosbag2 (`#2858 <https://github.com/ros2/rclcpp/issues/2858>`_) (`#2901 <https://github.com/ros2/rclcpp/issues/2901>`_)
+* Fujitatomoya/test append parameter override (`#2896 <https://github.com/ros2/rclcpp/issues/2896>`_) (`#2899 <https://github.com/ros2/rclcpp/issues/2899>`_)
+* Add overload of `append_parameter_override` (`#2891 <https://github.com/ros2/rclcpp/issues/2891>`_) (`#2894 <https://github.com/ros2/rclcpp/issues/2894>`_)
+* Contributors: Tim Clephas, mergify[bot]
+
+29.5.2 (2025-07-07)
+-------------------
+* Shutdown deadlock fix jazzy (`#2887 <https://github.com/ros2/rclcpp/issues/2887>`_) (`#2888 <https://github.com/ros2/rclcpp/issues/2888>`_)
+* Fix test_publisher_with_system_default_qos (`#2881 <https://github.com/ros2/rclcpp/issues/2881>`_) (`#2884 <https://github.com/ros2/rclcpp/issues/2884>`_)
+* Contributors: Tomoya Fujita, Janosch Machowinski
+
+29.5.1 (2025-06-23)
+-------------------
+* Removed warning test_qos (`#2859 <https://github.com/ros2/rclcpp/issues/2859>`_) (`#2873 <https://github.com/ros2/rclcpp/issues/2873>`_)
+* Fix for memory leaks in rclcpp::SerializedMessage (`#2861 <https://github.com/ros2/rclcpp/issues/2861>`_) (`#2863 <https://github.com/ros2/rclcpp/issues/2863>`_)
+* get_all_data_impl() does not handle null pointers properly, causing segmentation fault (backport `#2840 <https://github.com/ros2/rclcpp/issues/2840>`_) (`#2850 <https://github.com/ros2/rclcpp/issues/2850>`_)
+* Added missing chrono includes (`#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2855 <https://github.com/ros2/rclcpp/issues/2855>`_)
+* QoSInitialization::from_rmw does not validate invalid history policy values, leading to silent failures (`#2841 <https://github.com/ros2/rclcpp/issues/2841>`_) (`#2846 <https://github.com/ros2/rclcpp/issues/2846>`_)
+* Add missing 's' to 'NodeParametersInterface' in doc/comment (`#2831 <https://github.com/ros2/rclcpp/issues/2831>`_) (`#2833 <https://github.com/ros2/rclcpp/issues/2833>`_)
+* subordinate node consistent behavior and update docstring. (`#2822 <https://github.com/ros2/rclcpp/issues/2822>`_) (`#2830 <https://github.com/ros2/rclcpp/issues/2830>`_)
+* throws std::invalid_argument if ParameterEvent is NULL. (`#2814 <https://github.com/ros2/rclcpp/issues/2814>`_)
+* Removed clang warnings (`#2823 <https://github.com/ros2/rclcpp/issues/2823>`_)
+* Contributors: Alejandro Hernández Cordero, Tomoya Fujita, mergify[bot]
+
 29.5.0 (2025-04-18)
 -------------------
 * Fix a race condition (`#2819 <https://github.com/ros2/rclcpp/issues/2819>`_)

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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html).
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 ## 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://github
 
 ## 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/experimental/buffers/ring_buffer_implementation.hpp

@@ -255,9 +255,13 @@ private:
     std::vector<BufferT> result_vtr;
     result_vtr.reserve(size_);
     for (size_t id = 0; id < size_; ++id) {
-      result_vtr.emplace_back(
-        new typename is_std_unique_ptr<T>::Ptr_type(
-          *(ring_buffer_[(read_index_ + id) % capacity_])));
+      const auto & elem(ring_buffer_[(read_index_ + id) % capacity_]);
+      if (elem != nullptr) {
+        result_vtr.emplace_back(new typename is_std_unique_ptr<T>::Ptr_type(
+          *elem));
+      } else {
+        result_vtr.emplace_back(nullptr);
+      }
     }
     return result_vtr;
   }

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.hpp

@@ -1559,6 +1559,10 @@ public:
    * which has been created using an existing instance of this class, but which
    * has an additional sub-namespace (short for subordinate namespace)
    * associated with it.
+   * A subordinate node and an instance of this class share all the node interfaces
+   * such as `rclcpp::node_interfaces::NodeParametersInterface`.
+   * Subordinate nodes are primarily used to organize namespaces and provide a
+   * hierarchical structure, but they are not meant to be completely independent nodes.
    * The sub-namespace will extend the node's namespace for the purpose of
    * creating additional entities, such as Publishers, Subscriptions, Service
    * Clients and Servers, and so on.

rclcpp/include/rclcpp/node_impl.hpp

@@ -323,11 +323,9 @@ template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & parameter) const
 {
-  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
-
   rclcpp::Parameter parameter_variant;
 
-  bool result = get_parameter(sub_name, parameter_variant);
+  bool result = get_parameter(name, parameter_variant);
   if (result) {
     parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
   }
@@ -342,9 +340,7 @@ Node::get_parameter_or(
   ParameterT & parameter,
   const ParameterT & alternative_value) const
 {
-  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
-
-  bool got_parameter = get_parameter(sub_name, parameter);
+  bool got_parameter = get_parameter(name, parameter);
   if (!got_parameter) {
     parameter = alternative_value;
   }

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

@@ -199,6 +199,12 @@ init_tuple(NodeT & n)
     { \
       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
 // *INDENT-ON*

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -53,13 +53,17 @@ 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),
     topic_type_hash_(info.topic_type_hash)
   {
+    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());
   }
 

rclcpp/include/rclcpp/node_options.hpp

@@ -146,6 +146,14 @@ public:
     return *this;
   }
 
+  /// Append a single parameter override, parameter idiom style.
+  NodeOptions &
+  append_parameter_override(const rclcpp::Parameter & param)
+  {
+    this->parameter_overrides().push_back(param);
+    return *this;
+  }
+
   /// Return the use_global_arguments flag.
   RCLCPP_PUBLIC
   bool

rclcpp/include/rclcpp/publisher_options.hpp

@@ -123,6 +123,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

@@ -430,7 +430,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_options.hpp

@@ -167,11 +167,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/typesupport_helpers.hpp

@@ -29,26 +29,48 @@
 
 namespace rclcpp
 {
-/// Load the type support library for the given type.
-/**
- * \param[in] type The topic type, e.g. "std_msgs/msg/String"
- * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
- * \return A shared library
- */
+
+/// \brief Extract the package name, middle module, and type name from a full type string.
+/// \details This function takes a full type string (e.g., "std_msgs/msg/String") and extracts
+/// the package name, middle module (if any), and type name. The middle module is the part
+/// between the package name and the type name, which is typically used for message types.
+/// For example, for "std_msgs/msg/String", it returns ("std_msgs", "msg", "String").
+/// \param[in] full_type
+/// \throws std::runtime_error if the input full type string is malformed or does not follow the
+/// expected format.
+/// \return A tuple containing the package name, middle module (if any), and type name.
+RCLCPP_PUBLIC
+std::tuple<std::string, std::string, std::string>
+extract_type_identifier(const std::string & full_type);
+
+/// \brief Look for the library in the ament prefix paths and return the path to the type support
+/// library.
+/// \param[in] package_name The name of the package containing the type support library,
+/// e.g. "std_msgs".
+/// \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+/// \throws std::runtime_error if the library is not found.
+/// \return The path to the type support library.
+RCLCPP_PUBLIC
+std::string get_typesupport_library_path(
+  const std::string & package_name, const std::string & typesupport_identifier);
+
+/// \brief Load the type support library for the given type.
+/// \param[in] type The topic type, e.g. "std_msgs/msg/String"
+/// \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+/// \throws std::runtime_error if the library is not found or cannot be loaded.
+/// \return A shared library
 RCLCPP_PUBLIC
 std::shared_ptr<rcpputils::SharedLibrary>
 get_typesupport_library(const std::string & type, const std::string & typesupport_identifier);
 
-/// Extract the message type support handle from the library.
-/**
- * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
- *
- * \param[in] type The topic type, e.g. "std_msgs/msg/String"
- * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
- * \param[in] library The shared type support library
- * \throws std::runtime_error if the symbol of type not found in the library.
- * \return A message type support handle
- */
+/// \brief Extracts the message type support handle from the library.
+/// \note The library needs to match the topic type. The shared library must stay loaded for the
+/// lifetime of the result.
+/// \param[in] type The topic type, e.g. "std_msgs/msg/String"
+/// \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+/// \param[in] library The shared type support library
+/// \throws std::runtime_error if the symbol of type not found in the library.
+/// \return A message type support handle
 RCLCPP_PUBLIC
 const rosidl_message_type_support_t *
 get_message_typesupport_handle(
@@ -56,16 +78,14 @@ get_message_typesupport_handle(
   const std::string & typesupport_identifier,
   rcpputils::SharedLibrary & library);
 
-/// Extract the service type support handle from the library.
-/**
- * The library needs to match the service type. The shared library must stay loaded for the lifetime of the result.
- *
- * \param[in] type The service type, e.g. "std_srvs/srv/Empty"
- * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
- * \param[in] library The shared type support library
- * \throws std::runtime_error if the symbol of type not found in the library.
- * \return A service type support handle
- */
+/// \brief Extracts the service type support handle from the library.
+/// \note The library needs to match the service type. The shared library must stay loaded for the
+/// lifetime of the result.
+/// \param[in] type The service type, e.g. "std_srvs/srv/Empty"
+/// \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+/// \param[in] library The shared type support library
+/// \throws std::runtime_error if the symbol of type not found in the library.
+/// \return A service type support handle
 RCLCPP_PUBLIC
 const rosidl_service_type_support_t *
 get_service_typesupport_handle(
@@ -73,17 +93,14 @@ get_service_typesupport_handle(
   const std::string & typesupport_identifier,
   rcpputils::SharedLibrary & library);
 
-/// Extract the action type support handle from the library.
-/**
- * The library needs to match the action type. The shared library must stay loaded for the lifetime
- * of the result.
- *
- * \param[in] type The action type, e.g. "example_interfaces/action/Fibonacci"
- * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
- * \param[in] library The shared type support library
- * \throws std::runtime_error if the symbol of type not found in the library.
- * \return A action type support handle
- */
+/// \brief Extracts the action type support handle from the library.
+/// \note The library needs to match the action type. The shared library must stay loaded for the
+/// lifetime of the result.
+/// \param[in] type The action type, e.g. "example_interfaces/action/Fibonacci"
+/// \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
+/// \param[in] library The shared type support library
+/// \throws std::runtime_error if the symbol of type not found in the library.
+/// \return A action type support handle
 RCLCPP_PUBLIC
 const rosidl_action_type_support_t *
 get_action_typesupport_handle(

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

rclcpp/resource/logging.hpp.em

@@ -124,8 +124,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
-      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/callback_group.cpp

@@ -59,6 +59,7 @@ CallbackGroup::type() const
 size_t
 CallbackGroup::size() const
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   return
     subscription_ptrs_.size() +
     service_ptrs_.size() +

rclcpp/src/rclcpp/context.cpp

@@ -14,6 +14,7 @@
 
 #include "rclcpp/context.hpp"
 
+#include <map>
 #include <memory>
 #include <mutex>
 #include <sstream>
@@ -142,11 +143,52 @@ rclcpp_logging_output_handler(
 }
 }  // extern "C"
 
+/**
+ * Global storage for pre and post shutdown recursive mutexes.
+ * Note, this is a ABI compatibility hack.
+ */
+class MutexLookup
+{
+  std::mutex m;
+
+  struct MutexHolder
+  {
+    std::recursive_mutex on_shutdown_callbacks_mutex_;
+    std::recursive_mutex pre_shutdown_callbacks_mutex_;
+  };
+
+  std::map<const Context *, std::unique_ptr<MutexHolder>> mutexMap;
+
+public:
+  MutexHolder & getMutexes(const Context *forContext)
+  {
+    auto it = mutexMap.find(forContext);
+    if(it == mutexMap.end()) {
+      it = mutexMap.emplace(forContext, std::make_unique<MutexHolder>()).first;
+    }
+
+    return *(it->second);
+  }
+
+  /**
+   * Only supposed to be called on deletion of context
+   */
+  void removeMutexes(const Context *forContext)
+  {
+    mutexMap.erase(forContext);
+  }
+};
+
+MutexLookup mutexStorage;
+
 Context::Context()
 : rcl_context_(nullptr),
   shutdown_reason_(""),
   logging_mutex_(nullptr)
-{}
+{
+  // allocate mutexes
+  mutexStorage.getMutexes(this);
+}
 
 Context::~Context()
 {
@@ -165,6 +207,9 @@ Context::~Context()
   } catch (...) {
     RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()");
   }
+
+  // delete mutexes
+  mutexStorage.removeMutexes(this);
 }
 
 RCLCPP_LOCAL
@@ -310,9 +355,17 @@ Context::shutdown(const std::string & reason)
 
   // call each pre-shutdown callback
   {
-    std::lock_guard<std::mutex> lock{pre_shutdown_callbacks_mutex_};
-    for (const auto & callback : pre_shutdown_callbacks_) {
-      (*callback)();
+    std::lock_guard<std::recursive_mutex> lock{mutexStorage.getMutexes(
+        this).pre_shutdown_callbacks_mutex_};
+    // callbacks may delete other callbacks during the execution,
+    // therefore we need to save a copy and check before execution
+    // if the next callback is still present
+    auto cpy = pre_shutdown_callbacks_;
+    for (const auto & callback : cpy) {
+      auto it = std::find(pre_shutdown_callbacks_.begin(), pre_shutdown_callbacks_.end(), callback);
+      if(it != pre_shutdown_callbacks_.end()) {
+        (*callback)();
+      }
     }
   }
 
@@ -325,9 +378,17 @@ Context::shutdown(const std::string & reason)
   shutdown_reason_ = reason;
   // call each shutdown callback
   {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    for (const auto & callback : on_shutdown_callbacks_) {
-      (*callback)();
+    std::lock_guard<std::recursive_mutex> lock(mutexStorage.getMutexes(
+      this).on_shutdown_callbacks_mutex_);
+    // callbacks may delete other callbacks during the execution,
+    // therefore we need to save a copy and check before execution
+    // if the next callback is still present
+    auto cpy = on_shutdown_callbacks_;
+    for (const auto & callback : cpy) {
+      auto it = std::find(on_shutdown_callbacks_.begin(), on_shutdown_callbacks_.end(), callback);
+      if(it != on_shutdown_callbacks_.end()) {
+        (*callback)();
+      }
     }
   }
 
@@ -398,10 +459,12 @@ Context::add_shutdown_callback(
     shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
 
   if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
-    std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(mutexStorage.getMutexes(
+      this).pre_shutdown_callbacks_mutex_);
     pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   } else {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(mutexStorage.getMutexes(
+      this).on_shutdown_callbacks_mutex_);
     on_shutdown_callbacks_.emplace_back(callback_shared_ptr);
   }
 
@@ -421,7 +484,7 @@ Context::remove_shutdown_callback(
   }
 
   const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {
-      const std::lock_guard<std::mutex> lock(mutex);
+      const std::lock_guard<std::recursive_mutex> lock(mutex);
       auto iter = callback_vector.begin();
       for (; iter != callback_vector.end(); iter++) {
         if ((*iter).get() == callback_shared_ptr.get()) {
@@ -432,6 +495,7 @@ Context::remove_shutdown_callback(
         return false;
       }
       callback_vector.erase(iter);
+
       return true;
     };
 
@@ -439,9 +503,11 @@ Context::remove_shutdown_callback(
     shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
 
   if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
-    return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+    return remove_callback(mutexStorage.getMutexes(this).pre_shutdown_callbacks_mutex_,
+      pre_shutdown_callbacks_);
   } else {
-    return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
+    return remove_callback(mutexStorage.getMutexes(this).on_shutdown_callbacks_mutex_,
+      on_shutdown_callbacks_);
   }
 }
 
@@ -462,7 +528,7 @@ std::vector<rclcpp::Context::ShutdownCallback>
 Context::get_shutdown_callback() const
 {
   const auto get_callback_vector = [](auto & mutex, auto & callback_set) {
-      const std::lock_guard<std::mutex> lock(mutex);
+      const std::lock_guard<std::recursive_mutex> lock(mutex);
       std::vector<rclcpp::Context::ShutdownCallback> callbacks;
       for (auto & callback : callback_set) {
         callbacks.push_back(*callback);
@@ -474,9 +540,11 @@ Context::get_shutdown_callback() const
     shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
 
   if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
-    return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
+    return get_callback_vector(mutexStorage.getMutexes(this).pre_shutdown_callbacks_mutex_,
+      pre_shutdown_callbacks_);
   } else {
-    return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
+    return get_callback_vector(mutexStorage.getMutexes(this).on_shutdown_callbacks_mutex_,
+      on_shutdown_callbacks_);
   }
 }
 

rclcpp/src/rclcpp/node.cpp

@@ -689,7 +689,7 @@ Node::create_generic_client(
     node_base_,
     node_graph_,
     node_services_,
-    service_name,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     service_type,
     qos,
     group);

rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp

@@ -65,11 +65,11 @@ public:
   : logger_(node_logging->get_logger()),
     node_base_(node_base)
   {
+    rclcpp::ParameterValue enable_param;
     const std::string enable_param_name = "start_type_description_service";
 
-    bool enabled = false;
-    try {
-      auto enable_param = node_parameters->declare_parameter(
+    if (!node_parameters->has_parameter(enable_param_name)) {
+      enable_param = node_parameters->declare_parameter(
         enable_param_name,
         rclcpp::ParameterValue(true),
         rcl_interfaces::msg::ParameterDescriptor()
@@ -77,13 +77,21 @@ public:
         .set__type(rclcpp::PARAMETER_BOOL)
         .set__description("Start the ~/get_type_description service for this node.")
         .set__read_only(true));
-      enabled = enable_param.get<bool>();
-    } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) {
-      RCLCPP_ERROR(logger_, "%s", exc.what());
-      throw;
+    } else {
+      enable_param = node_parameters->get_parameter(enable_param_name).get_parameter_value();
+    }
+    if (enable_param.get_type() != rclcpp::PARAMETER_BOOL) {
+      RCLCPP_ERROR(
+        logger_,
+            "Invalid type '%s' for parameter 'start_type_description_service', should be 'bool'",
+        rclcpp::to_string(enable_param.get_type()).c_str());
+      std::ostringstream ss;
+      ss << "Wrong parameter type, parameter {" << enable_param_name << "} is of type {bool}, "
+         << "setting it to {" << to_string(enable_param.get_type()) << "} is not allowed.";
+      throw rclcpp::exceptions::InvalidParameterTypeException(enable_param_name, ss.str());
     }
 
-    if (enabled) {
+    if (enable_param.get<bool>()) {
       auto * rcl_node = node_base->get_rcl_node_handle();
       std::shared_ptr<rcl_service_t> rcl_srv(
         new rcl_service_t,

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,9 +47,9 @@ 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());
       } catch (const rclcpp::exceptions::ParameterUninitializedException & 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);
@@ -70,7 +70,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);
@@ -91,7 +91,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();
         }
@@ -119,7 +119,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";
@@ -139,7 +139,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

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

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/src/rclcpp/typesupport_helpers.cpp

@@ -35,62 +35,6 @@ namespace rclcpp
 namespace
 {
 
-// Look for the library in the ament prefix paths.
-std::string get_typesupport_library_path(
-  const std::string & package_name, const std::string & typesupport_identifier)
-{
-  const char * dynamic_library_folder;
-#ifdef _WIN32
-  dynamic_library_folder = "/bin/";
-#elif __APPLE__
-  dynamic_library_folder = "/lib/";
-#else
-  dynamic_library_folder = "/lib/";
-#endif
-
-  std::string package_prefix;
-  try {
-    package_prefix = ament_index_cpp::get_package_prefix(package_name);
-  } catch (ament_index_cpp::PackageNotFoundError & e) {
-    throw std::runtime_error(e.what());
-  }
-
-  const std::string library_path = rcpputils::path_for_library(
-    package_prefix + dynamic_library_folder,
-    package_name + "__" + typesupport_identifier);
-  if (library_path.empty()) {
-    throw std::runtime_error(
-            "Typesupport library for " + package_name + " does not exist in '" + package_prefix +
-            "'.");
-  }
-  return library_path;
-}
-
-std::tuple<std::string, std::string, std::string>
-extract_type_identifier(const std::string & full_type)
-{
-  char type_separator = '/';
-  auto sep_position_back = full_type.find_last_of(type_separator);
-  auto sep_position_front = full_type.find_first_of(type_separator);
-  if (sep_position_back == std::string::npos ||
-    sep_position_back == 0 ||
-    sep_position_back == full_type.length() - 1)
-  {
-    throw std::runtime_error(
-            "Message type is not of the form package/type and cannot be processed");
-  }
-
-  std::string package_name = full_type.substr(0, sep_position_front);
-  std::string middle_module = "";
-  if (sep_position_back - sep_position_front > 0) {
-    middle_module =
-      full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1);
-  }
-  std::string type_name = full_type.substr(sep_position_back + 1);
-
-  return std::make_tuple(package_name, middle_module, type_name);
-}
-
 const void * get_typesupport_handle_impl(
   const std::string & type,
   const std::string & typesupport_identifier,
@@ -131,6 +75,61 @@ const void * get_typesupport_handle_impl(
 
 }  // anonymous namespace
 
+std::tuple<std::string, std::string, std::string>
+extract_type_identifier(const std::string & full_type)
+{
+  char type_separator = '/';
+  auto sep_position_back = full_type.find_last_of(type_separator);
+  auto sep_position_front = full_type.find_first_of(type_separator);
+  if (sep_position_back == std::string::npos ||
+    sep_position_back == 0 ||
+    sep_position_back == full_type.length() - 1)
+  {
+    throw std::runtime_error(
+      "Message type is not of the form package/type and cannot be processed");
+  }
+
+  std::string package_name = full_type.substr(0, sep_position_front);
+  std::string middle_module = "";
+  if (sep_position_back - sep_position_front > 0) {
+    middle_module =
+      full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1);
+  }
+  std::string type_name = full_type.substr(sep_position_back + 1);
+
+  return std::make_tuple(package_name, middle_module, type_name);
+}
+
+std::string get_typesupport_library_path(
+  const std::string & package_name, const std::string & typesupport_identifier)
+{
+  const char * dynamic_library_folder;
+#ifdef _WIN32
+  dynamic_library_folder = "/bin/";
+#elif __APPLE__
+  dynamic_library_folder = "/lib/";
+#else
+  dynamic_library_folder = "/lib/";
+#endif
+
+  std::string package_prefix;
+  try {
+    package_prefix = ament_index_cpp::get_package_prefix(package_name);
+  } catch (ament_index_cpp::PackageNotFoundError & e) {
+    throw std::runtime_error(e.what());
+  }
+
+  const std::string library_path = rcpputils::path_for_library(
+    package_prefix + dynamic_library_folder,
+    package_name + "__" + typesupport_identifier);
+  if (library_path.empty()) {
+    throw std::runtime_error(
+      "Typesupport library for " + package_name + " does not exist in '" + package_prefix +
+        "'.");
+  }
+  return library_path;
+}
+
 std::shared_ptr<rcpputils::SharedLibrary>
 get_typesupport_library(const std::string & type, const std::string & typesupport_identifier)
 {

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/executors/test_events_queue.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
 
 #include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -81,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;
 };
 
 template<typename T>
@@ -152,7 +152,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);
 
@@ -263,7 +263,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([&]() {
@@ -300,7 +300,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(
@@ -344,7 +344,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);
@@ -586,7 +586,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

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -1,617 +0,0 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <chrono>
-#include <memory>
-#include <stdexcept>
-
-#include "rclcpp/rclcpp.hpp"
-
-#include "rcpputils/scope_exit.hpp"
-
-#include "test_msgs/msg/empty.hpp"
-#include "test_msgs/srv/empty.hpp"
-
-#include "../../mocking_utils/patch.hpp"
-#include "../../utils/rclcpp_gtest_macros.hpp"
-
-namespace
-{
-
-struct NumberOfEntities
-{
-  size_t subscriptions = 0;
-  size_t timers = 0;
-  size_t services = 0;
-  size_t clients = 0;
-  size_t waitables = 0;
-};
-
-std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
-{
-  auto number_of_entities = std::make_unique<NumberOfEntities>();
-  node->for_each_callback_group(
-    [&number_of_entities](rclcpp::CallbackGroup::SharedPtr group)
-    {
-      if (!group->can_be_taken_from().load()) {
-        return;
-      }
-      group->find_subscription_ptrs_if(
-        [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
-        {
-          number_of_entities->subscriptions++; return false;
-        });
-      group->find_timer_ptrs_if(
-        [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
-        {
-          number_of_entities->timers++; return false;
-        });
-      group->find_service_ptrs_if(
-        [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
-        {
-          number_of_entities->services++; return false;
-        });
-      group->find_client_ptrs_if(
-        [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
-        {
-          number_of_entities->clients++; return false;
-        });
-      group->find_waitable_ptrs_if(
-        [&number_of_entities](rclcpp::Waitable::SharedPtr &)
-        {
-          number_of_entities->waitables++; return false;
-        });
-    });
-
-  return number_of_entities;
-}
-
-}  // namespace
-
-class TestStaticExecutorEntitiesCollector : public ::testing::Test
-{
-public:
-  void SetUp()
-  {
-    rclcpp::init(0, nullptr);
-    entities_collector_ =
-      std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
-  }
-
-  void TearDown()
-  {
-    rclcpp::shutdown();
-  }
-
-  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
-};
-
-TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) {
-  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_waitables());
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
-
-  // Check adding second time
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_node(node1->get_node_base_interface()),
-    std::runtime_error("Node has already been added to an executor."));
-
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-  EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
-  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
-
-  EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
-  EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
-  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
-
-  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
-  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
-  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  // Check memory strategy is nullptr
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr;
-  EXPECT_THROW(
-    entities_collector_->init(&wait_set, memory_strategy),
-    std::runtime_error);
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  const auto expected_number_of_entities = get_number_of_default_entities(node);
-  EXPECT_NE(nullptr, expected_number_of_entities);
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-  EXPECT_EQ(
-    expected_number_of_entities->subscriptions,
-    entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services());
-  EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients());
-  // One extra for the executor
-  EXPECT_EQ(
-    1u + expected_number_of_entities->waitables,
-    entities_collector_->get_number_of_waitables());
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy);
-  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
-
-  // Still one for the executor
-  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
-  rclcpp::Context::SharedPtr shared_context = nullptr;
-  {
-    auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-    auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-    auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
-    entities_collector_->add_node(node1->get_node_base_interface());
-    entities_collector_->add_node(node2->get_node_base_interface());
-    entities_collector_->add_node(node3->get_node_base_interface());
-    shared_context = node1->get_node_base_interface()->get_context();
-  }
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  // Expect weak_node pointers to be cleaned up and used
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
-
-  // Still one for the executor
-  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
-}
-
-class TestWaitable : public rclcpp::Waitable
-{
-public:
-  void add_to_wait_set(rcl_wait_set_t &) override {}
-
-  bool is_ready(const rcl_wait_set_t &) override {return true;}
-
-  std::shared_ptr<void>
-  take_data() override
-  {
-    return nullptr;
-  }
-  void
-  execute(const std::shared_ptr<void> &) override {}
-};
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto expected_number_of_entities = get_number_of_default_entities(node);
-  EXPECT_NE(nullptr, expected_number_of_entities);
-
-  // Create 1 of each entity type
-  auto subscription =
-    node->create_subscription<test_msgs::msg::Empty>(
-    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {});
-  auto timer =
-    node->create_wall_timer(std::chrono::seconds(60), []() {});
-  auto service =
-    node->create_service<test_msgs::srv::Empty>(
-    "service",
-    [](
-      const test_msgs::srv::Empty::Request::SharedPtr,
-      test_msgs::srv::Empty::Response::SharedPtr) {});
-  auto client = node->create_client<test_msgs::srv::Empty>("service");
-  auto waitable = std::make_shared<TestWaitable>();
-
-  // Adding a subscription could add another waitable, so we need to get the
-  // current number of waitables just before adding the new waitable.
-  expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables;
-  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
-
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  EXPECT_EQ(
-    1u + expected_number_of_entities->subscriptions,
-    entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(
-    1u + expected_number_of_entities->services,
-    entities_collector_->get_number_of_services());
-  EXPECT_EQ(
-    1u + expected_number_of_entities->clients,
-    entities_collector_->get_number_of_clients());
-
-  // One extra for the executor
-  EXPECT_EQ(
-    2u + expected_number_of_entities->waitables,
-    entities_collector_->get_number_of_waitables());
-
-  entities_collector_->remove_node(node->get_node_base_interface());
-  entities_collector_->init(&wait_set, memory_strategy);
-  EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_services());
-  EXPECT_EQ(0u, entities_collector_->get_number_of_clients());
-  // Still one for the executor
-  EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_node(node->get_node_base_interface());
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
-    std::runtime_error("Callback group has already been added to an executor."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-  cb_group->get_associated_with_executor_atomic().exchange(false);
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
-    std::runtime_error("Callback group was already added to executor."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  {
-    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
-    std::shared_ptr<void> data = entities_collector_->take_data();
-    RCLCPP_EXPECT_THROW_EQ(
-      entities_collector_->execute(data),
-      std::runtime_error("Couldn't clear wait set"));
-  }
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  {
-    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
-    std::shared_ptr<void> data = entities_collector_->take_data();
-    RCLCPP_EXPECT_THROW_EQ(
-      entities_collector_->execute(data),
-      std::runtime_error("Couldn't resize the wait set: error not set"));
-  }
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) {
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
-    std::runtime_error("Couldn't clear wait set"));
-  rcl_reset_error();
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  {
-    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
-    RCLCPP_EXPECT_THROW_EQ(
-      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
-      std::runtime_error("rcl_wait() failed: error not set"));
-  }
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // Create 1 of each entity type
-  auto subscription =
-    node->create_subscription<test_msgs::msg::Empty>(
-    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {});
-  auto timer =
-    node->create_wall_timer(std::chrono::seconds(60), []() {});
-  auto service =
-    node->create_service<test_msgs::srv::Empty>(
-    "service",
-    [](
-      const test_msgs::srv::Empty::Request::SharedPtr,
-      test_msgs::srv::Empty::Response::SharedPtr) {});
-  auto client = node->create_client<test_msgs::srv::Empty>("service");
-  auto waitable = std::make_shared<TestWaitable>();
-
-  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
-
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  {
-    auto mock = mocking_utils::patch_and_return(
-      "lib:rclcpp", rcl_wait_set_add_subscription,
-      RCL_RET_ERROR);
-    RCLCPP_EXPECT_THROW_EQ(
-      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
-      std::runtime_error("Couldn't fill wait set"));
-  }
-
-  entities_collector_->remove_node(node->get_node_base_interface());
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 2u);
-
-  cb_group.reset();
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-
-  node.reset();
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->remove_callback_group(cb_group),
-    std::runtime_error("Node must not be deleted before its callback group(s)."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) {
-  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
-
-  entities_collector_->remove_callback_group(cb_group);
-
-  RCLCPP_EXPECT_THROW_EQ(
-    entities_collector_->remove_callback_group(cb_group),
-    std::runtime_error("Callback group needs to be associated with executor."));
-}
-
-TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
-
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
-
-  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
-}
-
-TEST_F(
-  TestStaticExecutorEntitiesCollector,
-  add_callback_groups_from_nodes_associated_to_executor_add) {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
-  entities_collector_->add_node(node->get_node_base_interface());
-
-  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_allocator_t allocator = rcl_get_default_allocator();
-  auto shared_context = node->get_node_base_interface()->get_context();
-  rcl_context_t * context = shared_context->get_rcl_context().get();
-  EXPECT_EQ(
-    RCL_RET_OK,
-    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
-  RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
-
-  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
-  rclcpp::GuardCondition guard_condition(shared_context);
-
-  entities_collector_->init(&wait_set, memory_strategy);
-  RCPPUTILS_SCOPE_EXIT(entities_collector_->fini());
-
-  cb_group->get_associated_with_executor_atomic().exchange(false);
-  std::shared_ptr<void> data = entities_collector_->take_data();
-  entities_collector_->execute(data);
-
-  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-}

rclcpp/test/rclcpp/executors/test_waitable.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <atomic>
+#include <chrono>
 #include <functional>
 #include <memory>
 #include <thread>

rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp

@@ -192,7 +192,7 @@ TEST_F(TestNodeInterfaces, ni_init) {
 /*
    Testing macro'ed getters.
  */
-TEST_F(TestNodeInterfaces, ni_all_init) {
+TEST_F(TestNodeInterfaces, ni_all_init_non_const) {
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
 
   using rclcpp::node_interfaces::NodeInterfaces;
@@ -252,3 +252,64 @@ TEST_F(TestNodeInterfaces, ni_all_init) {
     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/node_interfaces/test_node_parameters.cpp

@@ -128,7 +128,7 @@ TEST_F(TestNodeParameters, list_parameters)
     list_result5.names.end());
 }
 
-TEST_F(TestNodeParameters, parameter_overrides)
+TEST_F(TestNodeParameters, parameter_overrides_with_value)
 {
   rclcpp::NodeOptions node_options;
   node_options.automatically_declare_parameters_from_overrides(true);
@@ -146,6 +146,24 @@ TEST_F(TestNodeParameters, parameter_overrides)
   EXPECT_EQ(2u, parameter_overrides.size());
 }
 
+TEST_F(TestNodeParameters, parameter_overrides_with_parameter)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  node_options.append_parameter_override(rclcpp::Parameter("param1", true));
+  node_options.append_parameter_override(rclcpp::Parameter("param2", 42));
+
+  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns", node_options);
+
+  auto * node_parameters_interface =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node2->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters_interface);
+
+  const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
+  EXPECT_EQ(2u, parameter_overrides.size());
+}
+
 TEST_F(TestNodeParameters, set_parameters) {
   rclcpp::NodeOptions node_options;
   node_options.allow_undeclared_parameters(true);

rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp

@@ -37,6 +37,38 @@ TEST_F(TestNodeTypeDescriptions, interface_created)
   ASSERT_NE(nullptr, node.get_node_type_descriptions_interface());
 }
 
+TEST_F(TestNodeTypeDescriptions, automatically_declare_parameters_from_overrides)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  node_options.append_parameter_override("start_type_description_service", false);
+  ASSERT_NO_THROW(
+  {
+    auto node = std::make_shared<rclcpp::Node>("node", "ns", node_options);
+    (void) node;
+  });
+}
+
+TEST_F(TestNodeTypeDescriptions, bad_parameter_type)
+{
+  rclcpp::NodeOptions node_options;
+  node_options.append_parameter_override("start_type_description_service", "unexpected_type");
+
+  ASSERT_THROW(
+  {
+    node_options.automatically_declare_parameters_from_overrides(false);
+    auto node = std::make_shared<rclcpp::Node>("node", "ns", node_options);
+    (void) node;
+  }, rclcpp::exceptions::InvalidParameterTypeException);
+
+  ASSERT_THROW(
+  {
+    node_options.automatically_declare_parameters_from_overrides(true);
+    auto node = std::make_shared<rclcpp::Node>("node", "ns", node_options);
+    (void) node;
+  }, rclcpp::exceptions::InvalidParameterTypeException);
+}
+
 TEST_F(TestNodeTypeDescriptions, disabled_no_service)
 {
   rclcpp::NodeOptions node_options;

rclcpp/test/rclcpp/test_executor.cpp

@@ -342,12 +342,12 @@ TEST_F(TestExecutor, spin_all_fail_wait_set_clear) {
   subscription =
     rclcpp::create_subscription<test_msgs::msg::Empty>(
     node_topics, "test", rclcpp::QoS(10), std::move(callback));
-  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
 
-  dummy.spin_all(std::chrono::milliseconds(1));
-  // second spin_all triggers rcl_wait_set_clear that should be called
-  // whenever a waitset gets rebuild and it was not changed in size.
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
+    // the first spin might trigger the rebuild, but only the second triggers
+    // it for sure, therefore we spin two times
+    dummy.spin_all(std::chrono::milliseconds(1));
     dummy.spin_all(std::chrono::milliseconds(1)),
     std::runtime_error("Couldn't clear the wait set: error not set"));
 }

rclcpp/test/rclcpp/test_generic_client.cpp

@@ -221,7 +221,7 @@ TEST_F(TestGenericClient, wait_for_service) {
 TEST_F(TestGenericClientSub, construction_and_destruction) {
   {
     auto client = subnode->create_generic_client("test_service", "test_msgs/srv/Empty");
-    EXPECT_STREQ(client->get_service_name(), "/ns/test_service");
+    EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/test_service");
   }
 
   {

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

@@ -535,7 +535,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) {
@@ -587,9 +587,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) {
@@ -694,9 +694,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 message
  */
 TEST(TestIntraProcessManager, multiple_subscriptions_different_type) {
   using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager;

rclcpp/test/rclcpp/test_logger_service.cpp

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

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_node.cpp

@@ -306,6 +306,39 @@ TEST_F(TestNode, subnode_get_name_and_namespace) {
     }, rclcpp::exceptions::NameValidationError);
   }
 }
+
+TEST_F(TestNode, subnode_parameter_operation) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+  auto subnode = node->create_sub_node("sub_ns");
+
+  auto value = subnode->declare_parameter("param", 5);
+  EXPECT_EQ(value, 5);
+  // node and sub-node shares NodeParametersInterface, so expecting the exception.
+  EXPECT_THROW(
+    node->declare_parameter("param", 0),
+    rclcpp::exceptions::ParameterAlreadyDeclaredException);
+  rclcpp::Parameter param;
+
+  node->get_parameter("param", param);
+  EXPECT_EQ(param.get_value<int>(), 5);
+  subnode->get_parameter("param", param);
+  EXPECT_EQ(param.get_value<int>(), 5);
+
+  int param_int;
+  node->get_parameter("param", param_int);
+  EXPECT_EQ(param_int, 5);
+  subnode->get_parameter("param", param_int);
+  EXPECT_EQ(param_int, 5);
+
+  EXPECT_EQ(node->get_parameter_or("param", 333), 5);
+  EXPECT_EQ(subnode->get_parameter_or("param", 666), 5);
+
+  node->get_parameter_or("param", param_int, 333);
+  EXPECT_EQ(param_int, 5);
+  subnode->get_parameter_or("param", param_int, 666);
+  EXPECT_EQ(param_int, 5);
+}
+
 /*
    Testing node construction and destruction.
  */

rclcpp/test/rclcpp/test_node_options.cpp

@@ -258,8 +258,10 @@ TEST(TestNodeOptions, append_parameter_override) {
   auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false);
   rclcpp::Parameter parameter("some_parameter", 10);
   options.append_parameter_override("some_parameter", 10);
-  EXPECT_EQ(1u, options.parameter_overrides().size());
+  options.append_parameter_override(parameter);
+  EXPECT_EQ(2u, options.parameter_overrides().size());
   EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name());
+  EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[1].get_name());
 }
 
 TEST(TestNodeOptions, rcl_node_options_fini_error) {

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

@@ -199,7 +199,7 @@ TEST_F(TestPublisher, test_publisher_with_system_default_qos) {
   using test_msgs::msg::Empty;
   ASSERT_NO_THROW(
   {
-    auto publisher = node->create_publisher<Empty>("topic", rclcpp::SystemDefaultsQoS());
+    auto publisher = node->create_publisher<Empty>("topic", rclcpp::SystemDefaultsQoS(), options);
   });
 }
 

rclcpp/test/rclcpp/test_qos.cpp

@@ -272,3 +272,15 @@ TEST(TestQoS, qos_check_compatible)
     }
   }
 }
+
+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_ring_buffer_implementation.cpp

@@ -139,3 +139,15 @@ TEST(TestRingBufferImplementation, basic_usage_unique_ptr) {
   EXPECT_EQ(false, rb.has_data());
   EXPECT_EQ(false, rb.is_full());
 }
+
+TEST(TestRingBufferImplementation, handle_nullptr_deletion) {
+  rclcpp::experimental::buffers::RingBufferImplementation<std::unique_ptr<int>> rb(3);
+  rb.enqueue(std::make_unique<int>(42));
+  rb.enqueue(nullptr);  // intentionally enqueuing nullptr
+  rb.enqueue(std::make_unique<int>(84));
+  auto all_data = rb.get_all_data();
+  EXPECT_EQ(3u, all_data.size());
+  EXPECT_EQ(42, *(all_data[0]));
+  EXPECT_EQ(nullptr, all_data[1]);
+  EXPECT_EQ(84, *(all_data[2]));
+}

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_service_introspection.cpp

@@ -15,6 +15,7 @@
 #include <rcl/service_introspection.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <map>
 #include <string>
 

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,38 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+29.5.7 (2026-02-09)
+-------------------
+* Update exception documentation for goal cancellation in ServerGoalHandle (`#3019 <https://github.com/ros2/rclcpp/issues/3019>`_) (`#3022 <https://github.com/ros2/rclcpp/issues/3022>`_)
+* Contributors: mergify[bot]
+
+29.5.6 (2025-12-23)
+-------------------
+
+29.5.5 (2025-11-18)
+-------------------
+* Fix REP url locations (`#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2989 <https://github.com/ros2/rclcpp/issues/2989>`_)
+* Contributors: mergify[bot]
+
+29.5.4 (2025-10-21)
+-------------------
+* it misses the iterator second to lock the weakptr. (`#2958 <https://github.com/ros2/rclcpp/issues/2958>`_) (`#2959 <https://github.com/ros2/rclcpp/issues/2959>`_)
+* Contributors: mergify[bot]
+
+29.5.3 (2025-09-11)
+-------------------
+* fix cmake deprecation (`#2914 <https://github.com/ros2/rclcpp/issues/2914>`_) (`#2915 <https://github.com/ros2/rclcpp/issues/2915>`_)
+* Contributors: mergify[bot]
+
+29.5.2 (2025-07-07)
+-------------------
+
+29.5.1 (2025-06-23)
+-------------------
+* Replace std::default_random_engine with std::mt19937 (rolling) (`#2843 <https://github.com/ros2/rclcpp/issues/2843>`_) (`#2866 <https://github.com/ros2/rclcpp/issues/2866>`_)
+* Added missing chrono includes (`#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2855 <https://github.com/ros2/rclcpp/issues/2855>`_)
+* Contributors: mergify[bot]
+
 29.5.0 (2025-04-18)
 -------------------
 * Use std::recursive_mutex for action requests. (`#2798 <https://github.com/ros2/rclcpp/issues/2798>`_)

rclcpp_action/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.20)
 
 project(rclcpp_action)
 

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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html).
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 ## 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/client.hpp

@@ -350,7 +350,7 @@ public:
         return;
       }
 
-      goal_handle = it->lock();
+      goal_handle = it->second.lock();
     }
 
     if (goal_handle) {

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/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>29.5.0</version>
+  <version>29.5.7</version>
   <description>Adds action APIs for C++.</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_action/src/client_base.cpp

@@ -181,7 +181,7 @@ public:
   std::recursive_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(

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

@@ -67,7 +67,7 @@ TEST(TestActionTypes, goal_uuid_to_hashed_uuid_random) {
   // Use std::random_device to seed the generator of goal IDs.
   std::random_device rd;
   std::independent_bits_engine<
-    std::default_random_engine, 8, decltype(rd())> random_bytes_generator(rd());
+    std::mt19937, 8, decltype(rd())> random_bytes_generator(rd());
 
   std::vector<size_t> hashed_guuids;
   constexpr size_t iterations = 1000;

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,33 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+29.5.7 (2026-02-09)
+-------------------
+
+29.5.6 (2025-12-23)
+-------------------
+
+29.5.5 (2025-11-18)
+-------------------
+* Fix REP url locations (`#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2989 <https://github.com/ros2/rclcpp/issues/2989>`_)
+* Contributors: mergify[bot]
+
+29.5.4 (2025-10-21)
+-------------------
+
+29.5.3 (2025-09-11)
+-------------------
+* fix cmake deprecation (`#2914 <https://github.com/ros2/rclcpp/issues/2914>`_) (`#2915 <https://github.com/ros2/rclcpp/issues/2915>`_)
+* Contributors: mergify[bot]
+
+29.5.2 (2025-07-07)
+-------------------
+
+29.5.1 (2025-06-23)
+-------------------
+* Added missing chrono includes (`#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2855 <https://github.com/ros2/rclcpp/issues/2855>`_)
+* Contributors: mergify[bot]
+
 29.5.0 (2025-04-18)
 -------------------
 

rclcpp_components/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.20)
 
 project(rclcpp_components)
 

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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html).
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 ## 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,8 +13,19 @@
 # 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

@@ -59,15 +59,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})
 
   if(ARGS_NO_UNDEFINED_SYMBOLS AND WIN32)
     message(WARNING "NO_UNDEFINED_SYMBOLS is enabled for target \"${target}\", but this is unsupported on windows.")

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/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>29.5.0</version>
+  <version>29.5.7</version>
   <description>Package containing tools for dynamically loadable components</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</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/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,35 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+29.5.7 (2026-02-09)
+-------------------
+
+29.5.6 (2025-12-23)
+-------------------
+
+29.5.5 (2025-11-18)
+-------------------
+* Fix REP url locations (`#2987 <https://github.com/ros2/rclcpp/issues/2987>`_) (`#2989 <https://github.com/ros2/rclcpp/issues/2989>`_)
+* Add get_parameter_or overload returning value or alternative (`#2973 <https://github.com/ros2/rclcpp/issues/2973>`_) (`#2976 <https://github.com/ros2/rclcpp/issues/2976>`_)
+* Contributors: mergify[bot]
+
+29.5.4 (2025-10-21)
+-------------------
+
+29.5.3 (2025-09-11)
+-------------------
+* Clearer warning message, the old one lacked information and was perhaps misleading (`#2927 <https://github.com/ros2/rclcpp/issues/2927>`_) (`#2931 <https://github.com/ros2/rclcpp/issues/2931>`_)
+* fix cmake deprecation (`#2914 <https://github.com/ros2/rclcpp/issues/2914>`_) (`#2915 <https://github.com/ros2/rclcpp/issues/2915>`_)
+* Contributors: mergify[bot]
+
+29.5.2 (2025-07-07)
+-------------------
+
+29.5.1 (2025-06-23)
+-------------------
+* Added missing chrono includes (`#2854 <https://github.com/ros2/rclcpp/issues/2854>`_) (`#2855 <https://github.com/ros2/rclcpp/issues/2855>`_)
+* Contributors: mergify[bot]
+
 29.5.0 (2025-04-18)
 -------------------
 

rclcpp_lifecycle/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.20)
 
 project(rclcpp_lifecycle)
 

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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html).
+Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://reps.openrobotics.org/rep-2004/).
 
 ## 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

@@ -486,6 +486,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

@@ -306,5 +306,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/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>29.5.0</version>
+  <version>29.5.7</version>
   <description>Package containing a prototype for lifecycle implementation</description>
 
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>

rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp

@@ -390,6 +390,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
   constexpr bool publish_update = true;
   State initial_state;
   unsigned int current_state_id;
+  const rcl_lifecycle_transition_t * original_transition{nullptr};
 
   {
     std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
@@ -405,6 +406,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
     // 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)
@@ -455,12 +460,15 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
   // Update the internal current_state_
   current_state_ = State(state_machine_.current_state);
 
-  // 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);

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>
@@ -839,6 +840,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_service.cpp

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