9.2.2

Fix returning invalid namespace if sub_namespace is empty (#1658 ) (#1810 )
(cherry picked from commit 3cddb4edab) Signed-off-by: Markus Hofstaetter <markus.hofstaetter@ait.ac.at> Co-authored-by: M. Hofstätter <markus.hofstaetter@gmx.net>
2022-12-06 17:59:30 -08:00 · 2022-12-06 17:53:44 -08:00 · 2022-09-09 11:55:20 -07:00 · 2022-06-09 12:12:57 -07:00 · 2022-04-28 15:13:57 +00:00 · 2022-04-28 15:13:51 +00:00
51 changed files with 1581 additions and 332 deletions
--- a/README.md
+++ b/README.md
@@ -8,10 +8,10 @@ rclcpp provides the standard C++ API for interacting with ROS 2.

 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.

-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+Visit the [rclcpp API documentation](http://docs.ros2.org/galactic/api/rclcpp/) for a complete list of its main components.

 ### Examples

-The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
-and [Writing a simple service and client](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/galactic/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
+and [Writing a simple service and client](https://docs.ros.org/en/galactic/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html)
 contain some examples of rclcpp APIs in use.
--- a/rclcpp/CHANGELOG.rst
+++ b/rclcpp/CHANGELOG.rst
@@ -2,6 +2,40 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^

+9.2.2 (2022-12-06)
+------------------
+* Fix returning invalid namespace if sub_namespace is empty (`#1810 <https://github.com/ros2/rclcpp/issues/1810>`_)
+* use regex for wildcard matching (`#1987 <https://github.com/ros2/rclcpp/issues/1987>`_)
+* Add statistics for handle_loaned_message (`#1933 <https://github.com/ros2/rclcpp/issues/1933>`_)
+* Contributors: Aaron Lipinski, Barry Xu, Chen Lihui, M. Hofstätter
+
+9.2.1 (2022-04-28)
+------------------
+* Add test-dep ament_cmake_google_benchmark (`#1904 <https://github.com/ros2/rclcpp/issues/1904>`_) (`#1909 <https://github.com/ros2/rclcpp/issues/1909>`_)
+* Use parantheses around logging macro parameter (`#1820 <https://github.com/ros2/rclcpp/issues/1820>`_) (`#1822 <https://github.com/ros2/rclcpp/issues/1822>`_)
+* Contributors: mergify[bot]
+
+9.2.0 (2021-09-17)
+------------------
+* Added thread safe for_each_callback_group method (`#1741 <https://github.com/ros2/rclcpp/issues/1741>`_)
+* Added a function rclcpp::wait_for_message() convenience function (`#1705 <https://github.com/ros2/rclcpp/issues/1705>`_) (`#1740 <https://github.com/ros2/rclcpp/issues/1740>`_)
+* Fixed a documentation bug (`#1719 <https://github.com/ros2/rclcpp/issues/1719>`_) (`#1720 <https://github.com/ros2/rclcpp/issues/1720>`_)
+* Contributors: Aditya Pande, Karsten Knese, mergify[bot]
+
+9.1.0 (2021-05-20)
+------------------
+* Allow declaring uninitialized statically typed parameters. (`#1673 <https://github.com/ros2/rclcpp/issues/1673>`_) (`#1681 <https://github.com/ros2/rclcpp/issues/1681>`_)
+* [service] Don't use a weak_ptr to avoid leaking. (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_) (`#1670 <https://github.com/ros2/rclcpp/issues/1670>`_)
+* Contributors: Jacob Perron, Ivan Santiago Paunovic
+
+9.0.3 (2021-05-10)
+------------------
+* Use OnShutdown callback handle instead of OnShutdown callback (`#1639 <https://github.com/ros2/rclcpp/issues/1639>`_) (`#1650 <https://github.com/ros2/rclcpp/issues/1650>`_)
+* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (`#1643 <https://github.com/ros2/rclcpp/issues/1643>`_) (`#1644 <https://github.com/ros2/rclcpp/issues/1644>`_)
+* Increase cppcheck timeout to 500s (`#1634 <https://github.com/ros2/rclcpp/issues/1634>`_)
+* Clarify node parameters docs (`#1631 <https://github.com/ros2/rclcpp/issues/1631>`_)
+* Contributors: Barry Xu, Jacob Perron, Michel Hidalgo, Shane Loretz, William Woodall
+
 9.0.2 (2021-04-14)
 ------------------
 * Avoid returning loan when none was obtained. (`#1629 <https://github.com/ros2/rclcpp/issues/1629>`_)
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -23,6 +23,7 @@
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>

@@ -47,6 +48,17 @@ public:
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;

+class OnShutdownCallbackHandle
+{
+  friend class Context;
+
+public:
+  using OnShutdownCallbackType = std::function<void ()>;
+
+private:
+  std::weak_ptr<OnShutdownCallbackType> callback;
+};
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
 * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -177,7 +189,7 @@ public:
  bool
  shutdown(const std::string & reason);

-  using OnShutdownCallback = std::function<void ()>;
+  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;

  /// Add a on_shutdown callback to be called when shutdown is called for this context.
  /**
@@ -203,23 +215,47 @@ public:
  OnShutdownCallback
  on_shutdown(OnShutdownCallback callback);

-  /// Return the shutdown callbacks as const.
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
  /**
-   * Using the returned reference is not thread-safe with calls that modify
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * These callbacks will be called in the order they are added as the second
+   * to last step in shutdown().
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated singal handling thread.
+   *
+   * Also, shutdown() may be called from the destructor of this function.
+   * Therefore, it is not safe to throw exceptions from these callbacks.
+   * Instead, log errors or use some other mechanism to indicate an error has
+   * occurred.
+   *
+   * On shutdown callbacks may be registered before init and after shutdown,
+   * and persist on repeated init's.
+   *
+   * \param[in] callback the on_shutdown callback to be registered
+   * \return the created callback handle
   */
  RCLCPP_PUBLIC
-  const std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks() const;
+  virtual
+  OnShutdownCallbackHandle
+  add_on_shutdown_callback(OnShutdownCallback callback);
+
+  /// Remove an registered on_shutdown callbacks.
+  /**
+   * \param[in] callback_handle the on_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);

  /// Return the shutdown callbacks.
  /**
-   * Using the returned reference is not thread-safe with calls that modify
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * Returned callbacks are a copy of the registered callbacks.
   */
  RCLCPP_PUBLIC
-  std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks();
+  std::vector<OnShutdownCallback>
+  get_on_shutdown_callbacks() const;

  /// Return the internal rcl context.
  RCLCPP_PUBLIC
@@ -299,8 +335,8 @@ private:
  // attempt to acquire another sub context.
  std::recursive_mutex sub_contexts_mutex_;

-  std::vector<OnShutdownCallback> on_shutdown_callbacks_;
-  std::mutex on_shutdown_callbacks_mutex_;
+  std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  mutable std::mutex on_shutdown_callbacks_mutex_;

  /// Condition variable for timed sleep (see sleep_for).
  std::condition_variable interrupt_condition_variable_;
--- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp
+++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp
@@ -282,8 +282,8 @@ class ParameterModifiedInCallbackException : public std::runtime_error
  using std::runtime_error::runtime_error;
 };

-/// Thrown when a parameter override wasn't provided and one was required.
-class NoParameterOverrideProvided : public std::runtime_error
+/// Thrown when an uninitialized parameter is accessed.
+class ParameterUninitializedException : public std::runtime_error
 {
 public:
  /// Construct an instance.
@@ -291,8 +291,8 @@ public:
   * \param[in] name the name of the parameter.
   * \param[in] message custom exception message.
   */
-  explicit NoParameterOverrideProvided(const std::string & name)
-  : std::runtime_error("parameter '" + name + "' requires an user provided parameter override")
+  explicit ParameterUninitializedException(const std::string & name)
+  : std::runtime_error("parameter '" + name + "' is not initialized")
  {}
 };

--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -30,6 +30,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"

+#include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
@@ -571,6 +572,9 @@ protected:
  /// nodes that are associated with the executor
  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// shutdown callback handle registered to Context
+  rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
 };

 }  // namespace rclcpp
--- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
+++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp
@@ -202,7 +202,8 @@ public:
      // None of the buffers require ownership, so we promote the pointer
      std::shared_ptr<MessageT> msg = std::move(message);

-      this->template add_shared_msg_to_buffers<MessageT>(msg, sub_ids.take_shared_subscriptions);
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        msg, sub_ids.take_shared_subscriptions);
    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
      sub_ids.take_shared_subscriptions.size() <= 1)
    {
@@ -227,7 +228,7 @@ public:
      // for the buffers that do not require ownership
      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);

-      this->template add_shared_msg_to_buffers<MessageT>(
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
        shared_msg, sub_ids.take_shared_subscriptions);
      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
@@ -263,7 +264,7 @@ public:
      // If there are no owning, just convert to shared.
      std::shared_ptr<MessageT> shared_msg = std::move(message);
      if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
          shared_msg, sub_ids.take_shared_subscriptions);
      }
      return shared_msg;
@@ -273,7 +274,7 @@ public:
      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);

      if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
          shared_msg,
          sub_ids.take_shared_subscriptions);
      }
@@ -350,7 +351,10 @@ private:
  bool
  can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;

-  template<typename MessageT>
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter>
  void
  add_shared_msg_to_buffers(
    std::shared_ptr<const MessageT> message,
@@ -363,9 +367,16 @@ private:
      }
      auto subscription_base = subscription_it->second.subscription.lock();
      if (subscription_base) {
-        auto subscription = std::static_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+        auto subscription = std::dynamic_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
          >(subscription_base);
+        if (nullptr == subscription) {
+          throw std::runtime_error(
+                  "failed to dynamic cast SubscriptionIntraProcessBase to "
+                  "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
+                  "can happen when the publisher and subscription use different "
+                  "allocator types, which is not supported");
+        }

        subscription->provide_intra_process_message(message);
      } else {
@@ -394,9 +405,16 @@ private:
      }
      auto subscription_base = subscription_it->second.subscription.lock();
      if (subscription_base) {
-        auto subscription = std::static_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+        auto subscription = std::dynamic_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
          >(subscription_base);
+        if (nullptr == subscription) {
+          throw std::runtime_error(
+                  "failed to dynamic cast SubscriptionIntraProcessBase to "
+                  "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
+                  "can happen when the publisher and subscription use different "
+                  "allocator types, which is not supported");
+        }

        if (std::next(it) == subscription_ids.end()) {
          // If this is the last subscription, give up ownership
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -153,6 +153,17 @@ public:
  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
  get_callback_groups() const;

+  /// Iterate over the callback groups in the node, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
+  RCLCPP_PUBLIC
+  void
+  for_each_callback_group(const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
+
  /// Create and return a Publisher.
  /**
   * The rclcpp::QoS has several convenient constructors, including a
@@ -984,12 +995,15 @@ public:
  std::map<std::string, std::vector<std::string>>
  get_service_names_and_types() const;

-  /// Return the number of publishers that are advertised on a given topic.
+  /// Return a map of existing service names to list of service types for a specific node.
  /**
-   * \param[in] node_name the node_name on which to count the publishers.
-   * \param[in] namespace_ the namespace of the node associated with the name
-   * \return number of publishers that are advertised on a given topic.
-   * \throws std::runtime_error if publishers could not be counted
+   * This function only considers services - not clients.
+   * The returned names are the actual names used and do not have remap rules applied.
+   *
+   * \param[in] node_name name of the node.
+   * \param[in] namespace_ namespace of the node.
+   * \return a map of existing service names to list of service types.
+   * \throws std::runtime_error anything that rcl_error can throw.
   */
  RCLCPP_PUBLIC
  std::map<std::string, std::vector<std::string>>
--- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp
@@ -16,10 +16,13 @@
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_

 #include <memory>
+#include <mutex>
 #include <string>
+#include <unordered_map>
 #include <vector>

 #include "rcl/node.h"
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -30,12 +33,34 @@ namespace rclcpp
 namespace node_interfaces
 {

+RCLCPP_PUBLIC
+void global_for_each_callback_group(
+  NodeBaseInterface * node_base_interface,
+  const NodeBaseInterface::CallbackGroupFunction & func);
+
+// Class to hold the global map of mutexes
+class map_of_mutexes final
+{
+public:
+  // Methods need to be protected by internal mutex
+  void create_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+  std::shared_ptr<std::mutex>
+  get_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+  void delete_mutex_of_nodebase(const NodeBaseInterface * nodebase);
+
+private:
+  std::unordered_map<const NodeBaseInterface *, std::shared_ptr<std::mutex>> data_;
+  std::mutex internal_mutex_;
+};
+
 /// Implementation of the NodeBase part of the Node API.
 class NodeBase : public NodeBaseInterface
 {
 public:
  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)

+  static map_of_mutexes map_object;
+
  RCLCPP_PUBLIC
  NodeBase(
    const std::string & node_name,
--- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
+++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
@@ -38,6 +38,8 @@ class NodeBaseInterface
 public:
  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)

+  using CallbackGroupFunction = std::function<void (rclcpp::CallbackGroup::SharedPtr)>;
+
  RCLCPP_PUBLIC
  virtual
  ~NodeBaseInterface() = default;
--- a/rclcpp/include/rclcpp/parameter_map.hpp
+++ b/rclcpp/include/rclcpp/parameter_map.hpp
@@ -41,6 +41,16 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);

+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
+
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -177,25 +177,16 @@ public:
    using rosidl_typesupport_cpp::get_service_type_support_handle;
    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();

-    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
    // rcl does the static memory allocation here
    service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [weak_node_handle, service_name](rcl_service_t * service)
+      new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
      {
-        auto handle = weak_node_handle.lock();
-        if (handle) {
-          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
-            RCLCPP_ERROR(
-              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
-              "Error in destruction of rcl service handle: %s",
-              rcl_get_error_string().str);
-            rcl_reset_error();
-          }
-        } else {
-          RCLCPP_ERROR_STREAM(
-            rclcpp::get_logger("rclcpp"),
-            "Error in destruction of rcl service handle " << service_name <<
-              ": the Node Handle was destructed too early. You will leak memory");
+        if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl service handle: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
        }
        delete service;
      });
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -295,11 +295,31 @@ public:
    void * loaned_message,
    const rclcpp::MessageInfo & message_info) override
  {
+    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
+      // In this case, the message will be delivered via intra process and
+      // we should ignore this copy of the message.
+      return;
+    }
+
    auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
    // message is loaned, so we have to make sure that the deleter does not deallocate the message
    auto sptr = std::shared_ptr<CallbackMessageT>(
      typed_message, [](CallbackMessageT * msg) {(void) msg;});
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
    any_callback_.dispatch(sptr, message_info);
+
+    if (subscription_topic_statistics_) {
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
+      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
+      subscription_topic_statistics_->handle_message(*typed_message, time);
+    }
  }

  /// Return the borrowed message.
--- a/rclcpp/include/rclcpp/wait_for_message.hpp
+++ b/rclcpp/include/rclcpp/wait_for_message.hpp
@@ -0,0 +1,100 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
+#define RCLCPP__WAIT_FOR_MESSAGE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set.hpp"
+
+namespace rclcpp
+{
+/// Wait for the next incoming message.
+/**
+ * Given an already initialized subscription,
+ * wait for the next incoming message to arrive before the specified timeout.
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] subscription shared pointer to a previously initialized subscription.
+ * \param[in] context shared pointer to a context to watch for SIGINT requests.
+ * \param[in] time_to_wait parameter specifying the timeout before returning.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription,
+  std::shared_ptr<rclcpp::Context> context,
+  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+{
+  auto gc = std::make_shared<rclcpp::GuardCondition>(context);
+  auto shutdown_callback_handle = context->add_on_shutdown_callback(
+    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{gc}]() {
+      auto strong_gc = weak_gc.lock();
+      if (strong_gc) {
+        strong_gc->trigger();
+      }
+    });
+
+  rclcpp::WaitSet wait_set;
+  wait_set.add_subscription(subscription);
+  wait_set.add_guard_condition(gc);
+  auto ret = wait_set.wait(time_to_wait);
+  if (ret.kind() != rclcpp::WaitResultKind::Ready) {
+    return false;
+  }
+
+  if (wait_set.get_rcl_wait_set().guard_conditions[0]) {
+    return false;
+  }
+
+  rclcpp::MessageInfo info;
+  if (!subscription->take(out, info)) {
+    return false;
+  }
+
+  return true;
+}
+
+/// Wait for the next incoming 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[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.
+ * \return true if a message was successfully received, false if message could not
+ * be obtained or shutdown was triggered asynchronously on the context.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+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))
+{
+  auto sub = node->create_subscription<MsgT>(topic, 1, [](const std::shared_ptr<MsgT>) {});
+  return wait_for_message<MsgT, Rep, Period>(
+    out, sub, node->get_node_options().context(), time_to_wait);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_FOR_MESSAGE_HPP_
--- a/rclcpp/package.xml
+++ b/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>9.0.2</version>
+  <version>9.2.2</version>
  <description>The ROS client library in C++.</description>
  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
@@ -37,6 +37,7 @@
  <depend>tracetools</depend>

  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
--- a/rclcpp/resource/logging.hpp.em
+++ b/rclcpp/resource/logging.hpp.em
@@ -149,7 +149,7 @@ def get_rclcpp_suffix_from_features(features):
@[ if params]@
@(''.join(['      ' + p + ', \\\n' for p in params if p != stream_arg]))@
@[ end if]@
-      logger.get_name(), \
+      (logger).get_name(), \
@[ if 'stream' not in feature_combination]@
      __VA_ARGS__); \
@[ else]@
--- a/rclcpp/src/rclcpp/context.cpp
+++ b/rclcpp/src/rclcpp/context.cpp
@@ -315,9 +315,13 @@ Context::shutdown(const std::string & reason)
  // set shutdown reason
  shutdown_reason_ = reason;
  // call each shutdown callback
-  for (const auto & callback : on_shutdown_callbacks_) {
-    callback();
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    for (const auto & callback : on_shutdown_callbacks_) {
+      (*callback)();
+    }
  }
+
  // interrupt all blocking sleep_for() and all blocking executors or wait sets
  this->interrupt_all_sleep_for();
  // remove self from the global contexts
@@ -344,20 +348,48 @@ Context::shutdown(const std::string & reason)
 rclcpp::Context::OnShutdownCallback
 Context::on_shutdown(OnShutdownCallback callback)
 {
-  on_shutdown_callbacks_.push_back(callback);
+  add_on_shutdown_callback(callback);
  return callback;
 }

-const std::vector<rclcpp::Context::OnShutdownCallback> &
-Context::get_on_shutdown_callbacks() const
+rclcpp::OnShutdownCallbackHandle
+Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  return on_shutdown_callbacks_;
+  auto callback_shared_ptr = std::make_shared<OnShutdownCallback>(callback);
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    on_shutdown_callbacks_.emplace(callback_shared_ptr);
+  }
+
+  OnShutdownCallbackHandle callback_handle;
+  callback_handle.callback = callback_shared_ptr;
+  return callback_handle;
 }

-std::vector<rclcpp::Context::OnShutdownCallback> &
-Context::get_on_shutdown_callbacks()
+bool
+Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  return on_shutdown_callbacks_;
+  std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+  auto callback_shared_ptr = callback_handle.callback.lock();
+  if (callback_shared_ptr == nullptr) {
+    return false;
+  }
+  return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1;
+}
+
+std::vector<rclcpp::Context::OnShutdownCallback>
+Context::get_on_shutdown_callbacks() const
+{
+  std::vector<OnShutdownCallback> callbacks;
+
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    for (auto & iter : on_shutdown_callbacks_) {
+      callbacks.emplace_back(*iter);
+    }
+  }
+
+  return callbacks;
 }

 std::shared_ptr<rcl_context_t>
--- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp
+++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp
@@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides(
        [params]() {
          rcl_yaml_node_struct_fini(params);
        });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());

-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            result[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
+      if (initial_map.count(node_fqn) > 0) {
+        // Combine parameter yaml files, overwriting values in older ones
+        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
+          result[param.get_name()] =
+            rclcpp::ParameterValue(param.get_value_message());
        }
      }
    }
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -30,6 +30,7 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"

 #include "rcutils/logging_macros.h"

@@ -56,7 +57,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
  }

-  context_->on_shutdown(
+  shutdown_callback_handle_ = context_->add_on_shutdown_callback(
    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {
      auto strong_gc = weak_gc.lock();
      if (strong_gc) {
@@ -138,6 +139,14 @@ Executor::~Executor()
  }
  // Remove and release the sigint guard condition
  memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
+
+  // Remove shutdown callback handle registered to Context
+  if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to remove registered on_shutdown callback");
+    rcl_reset_error();
+  }
 }

 std::vector<rclcpp::CallbackGroup::WeakPtr>
@@ -182,14 +191,13 @@ Executor::add_callback_groups_from_nodes_associated_to_executor()
  for (auto & weak_node : weak_nodes_) {
    auto node = weak_node.lock();
    if (node) {
-      auto group_ptrs = node->get_callback_groups();
-      std::for_each(
-        group_ptrs.begin(), group_ptrs.end(),
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+      rclcpp::node_interfaces::global_for_each_callback_group(
+        node.get(),
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
        {
-          auto shared_group_ptr = group_ptr.lock();
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
+          if (
+            shared_group_ptr->automatically_add_to_executor_with_node() &&
+            !shared_group_ptr->get_associated_with_executor_atomic().load())
          {
            this->add_callback_group_to_map(
              shared_group_ptr,
@@ -263,18 +271,21 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
    throw std::runtime_error("Node has already been added to an executor.");
  }
  std::lock_guard<std::mutex> guard{mutex_};
-  for (auto & weak_group : node_ptr->get_callback_groups()) {
-    auto group_ptr = weak_group.lock();
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
-      group_ptr->automatically_add_to_executor_with_node())
+  rclcpp::node_interfaces::global_for_each_callback_group(
+    node_ptr.get(),
+    [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr)
    {
-      this->add_callback_group_to_map(
-        group_ptr,
-        node_ptr,
-        weak_groups_to_nodes_associated_with_executor_,
-        notify);
-    }
-  }
+      if (!group_ptr->get_associated_with_executor_atomic().load() &&
+      group_ptr->automatically_add_to_executor_with_node())
+      {
+        this->add_callback_group_to_map(
+          group_ptr,
+          node_ptr,
+          weak_groups_to_nodes_associated_with_executor_,
+          notify);
+      }
+    });
+
  weak_nodes_.push_back(node_ptr);
 }

--- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp
+++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp
@@ -23,6 +23,7 @@

 #include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"

 using rclcpp::executors::StaticExecutorEntitiesCollector;

@@ -290,18 +291,21 @@ StaticExecutorEntitiesCollector::add_node(
  if (has_executor.exchange(true)) {
    throw std::runtime_error("Node has already been added to an executor.");
  }
-  for (const auto & weak_group : node_ptr->get_callback_groups()) {
-    auto group_ptr = weak_group.lock();
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
-      group_ptr->automatically_add_to_executor_with_node())
+  rclcpp::node_interfaces::global_for_each_callback_group(
+    node_ptr.get(),
+    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
    {
-      is_new_node = (add_callback_group(
+      if (
+        !group_ptr->get_associated_with_executor_atomic().load() &&
+        group_ptr->automatically_add_to_executor_with_node())
+      {
+        is_new_node = (add_callback_group(
          group_ptr,
          node_ptr,
          weak_groups_to_nodes_associated_with_executor_) ||
        is_new_node);
-    }
-  }
+      }
+    });
  weak_nodes_.push_back(node_ptr);
  return is_new_node;
 }
@@ -467,13 +471,11 @@ StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_ex
  for (const auto & weak_node : weak_nodes_) {
    auto node = weak_node.lock();
    if (node) {
-      auto group_ptrs = node->get_callback_groups();
-      std::for_each(
-        group_ptrs.begin(), group_ptrs.end(),
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
+      rclcpp::node_interfaces::global_for_each_callback_group(
+        node.get(),
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
        {
-          auto shared_group_ptr = group_ptr.lock();
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
+          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
          !shared_group_ptr->get_associated_with_executor_atomic().load())
          {
            add_callback_group(
--- a/rclcpp/src/rclcpp/node.cpp
+++ b/rclcpp/src/rclcpp/node.cpp
@@ -61,6 +61,12 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri
            extension.c_str(),
            "a sub-namespace should not have a leading /",
            0);
+  } else if (existing_sub_namespace.empty() && extension.empty()) {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "sub-nodes should not extend nodes by an empty sub-namespace",
+            0);
  }

  std::string new_sub_namespace;
@@ -86,7 +92,11 @@ create_effective_namespace(const std::string & node_namespace, const std::string
  // and do not need trimming of `/` and other things, as they were validated
  // in other functions already.

-  if (node_namespace.back() == '/') {
+  // A node may not have a sub_namespace if it is no sub_node. In this case,
+  // just return the original namespace
+  if (sub_namespace.empty()) {
+    return node_namespace;
+  } else if (node_namespace.back() == '/') {
    // this is the special case where node_namespace is just `/`
    return node_namespace + sub_namespace;
  } else {
@@ -605,3 +615,9 @@ Node::get_node_options() const
 {
  return this->node_options_;
 }
+
+void Node::for_each_callback_group(
+  const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
+{
+  rclcpp::node_interfaces::global_for_each_callback_group(node_base_.get(), func);
+}
--- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp
@@ -31,6 +31,8 @@ using rclcpp::exceptions::throw_from_rcl_error;

 using rclcpp::node_interfaces::NodeBase;

+using rclcpp::node_interfaces::map_of_mutexes;
+
 NodeBase::NodeBase(
  const std::string & node_name,
  const std::string & namespace_,
@@ -46,6 +48,9 @@ NodeBase::NodeBase(
  associated_with_executor_(false),
  notify_guard_condition_is_valid_(false)
 {
+  // Generate a mutex for this instance of NodeBase
+  NodeBase::map_object.create_mutex_of_nodebase(this);
+
  // Setup the guard condition that is notified when changes occur in the graph.
  rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
  rcl_ret_t ret = rcl_guard_condition_init(
@@ -166,6 +171,8 @@ NodeBase::~NodeBase()
        "failed to destroy guard condition: %s", rcl_get_error_string().str);
    }
  }
+
+  NodeBase::map_object.delete_mutex_of_nodebase(this);
 }

 const char *
@@ -221,12 +228,11 @@ NodeBase::create_callback_group(
  rclcpp::CallbackGroupType group_type,
  bool automatically_add_to_executor_with_node)
 {
-  using rclcpp::CallbackGroup;
-  using rclcpp::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(
-    new CallbackGroup(
-      group_type,
-      automatically_add_to_executor_with_node));
+  auto group = std::make_shared<rclcpp::CallbackGroup>(
+    group_type,
+    automatically_add_to_executor_with_node);
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(this);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
  callback_groups_.push_back(group);
  return group;
 }
@@ -240,14 +246,16 @@ NodeBase::get_default_callback_group()
 bool
 NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group)
 {
-  bool group_belongs_to_this_node = false;
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(this);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
+
  for (auto & weak_group : this->callback_groups_) {
    auto cur_group = weak_group.lock();
    if (cur_group && (cur_group == group)) {
-      group_belongs_to_this_node = true;
+      return true;
    }
  }
-  return group_belongs_to_this_node;
+  return false;
 }

 const std::vector<rclcpp::CallbackGroup::WeakPtr> &
@@ -310,3 +318,41 @@ NodeBase::resolve_topic_or_service_name(
  allocator.deallocate(output_cstr, allocator.state);
  return output;
 }
+
+map_of_mutexes NodeBase::map_object = map_of_mutexes();
+
+void map_of_mutexes::create_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  this->data_.emplace(nodebase, std::make_shared<std::mutex>() );
+}
+
+std::shared_ptr<std::mutex> map_of_mutexes::get_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  return this->data_[nodebase];
+}
+
+void map_of_mutexes::delete_mutex_of_nodebase(
+  const rclcpp::node_interfaces::NodeBaseInterface * nodebase)
+{
+  std::lock_guard<std::mutex> guard(this->internal_mutex_);
+  this->data_.erase(nodebase);
+}
+
+// For each callback group free function implementation
+void rclcpp::node_interfaces::global_for_each_callback_group(
+  NodeBaseInterface * node_base_interface, const NodeBaseInterface::CallbackGroupFunction & func)
+{
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(node_base_interface);
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
+
+  for (const auto & weak_group : node_base_interface->get_callback_groups()) {
+    auto group = weak_group.lock();
+    if (group) {
+      func(group);
+    }
+  }
+}
--- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
@@ -349,6 +349,21 @@ __declare_parameter_common(
    initial_value = &overrides_it->second;
  }

+  // If there is no initial value, then skip initialization
+  if (initial_value->get_type() == rclcpp::PARAMETER_NOT_SET) {
+    // Add declared parameters to storage (without a value)
+    parameter_infos[name].descriptor.name = name;
+    if (parameter_descriptor.dynamic_typing) {
+      parameter_infos[name].descriptor.type = rclcpp::PARAMETER_NOT_SET;
+    } else {
+      parameter_infos[name].descriptor.type = parameter_descriptor.type;
+    }
+    parameters_out[name] = parameter_infos.at(name);
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    return result;
+  }
+
  // Check with the user's callback to see if the initial value can be set.
  std::vector<rclcpp::Parameter> parameter_wrappers {rclcpp::Parameter(name, *initial_value)};
  // This function also takes care of default vs initial value.
@@ -413,14 +428,6 @@ declare_parameter_helper(
    parameter_descriptor.type = static_cast<uint8_t>(type);
  }

-  if (
-    rclcpp::PARAMETER_NOT_SET == default_value.get_type() &&
-    overrides.find(name) == overrides.end() &&
-    parameter_descriptor.dynamic_typing == false)
-  {
-    throw rclcpp::exceptions::NoParameterOverrideProvided(name);
-  }
-
  rcl_interfaces::msg::ParameterEvent parameter_event;
  auto result = __declare_parameter_common(
    name,
@@ -806,14 +813,21 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
 rclcpp::Parameter
 NodeParameters::get_parameter(const std::string & name) const
 {
-  rclcpp::Parameter parameter;
+  std::lock_guard<std::recursive_mutex> lock(mutex_);

-  if (get_parameter(name, parameter)) {
-    return parameter;
+  auto param_iter = parameters_.find(name);
+  if (
+    parameters_.end() != param_iter &&
+    (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET ||
+    param_iter->second.descriptor.dynamic_typing))
+  {
+    return rclcpp::Parameter{name, param_iter->second.value};
  } else if (this->allow_undeclared_) {
-    return parameter;
-  } else {
+    return rclcpp::Parameter{};
+  } else if (parameters_.end() == param_iter) {
    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
+  } else {
+    throw rclcpp::exceptions::ParameterUninitializedException(name);
  }
 }

--- a/rclcpp/src/rclcpp/parameter_map.cpp
+++ b/rclcpp/src/rclcpp/parameter_map.cpp
@@ -13,8 +13,10 @@
 // limitations under the License.

 #include <string>
+#include <regex>
 #include <vector>

+#include "rcpputils/find_and_replace.hpp"
 #include "rclcpp/parameter_map.hpp"

 using rclcpp::exceptions::InvalidParametersException;
@@ -24,6 +26,12 @@ using rclcpp::ParameterValue;

 ParameterMap
 rclcpp::parameter_map_from(const rcl_params_t * const c_params)
+{
+  return parameter_map_from(c_params, nullptr);
+}
+
+ParameterMap
+rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn)
 {
  if (NULL == c_params) {
    throw InvalidParametersException("parameters struct is NULL");
@@ -49,6 +57,17 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
      node_name = c_node_name;
    }

+    if (node_fqn) {
+      // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"]
+      std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)");
+      if (!std::regex_match(node_fqn, std::regex(regex))) {
+        // No need to parse the items because the user just care about node_fqn
+        continue;
+      }
+
+      node_name = node_fqn;
+    }
+
    const rcl_node_params_t * const c_params_node = &(c_params->params[n]);

    std::vector<Parameter> & params_node = parameters[node_name];
@@ -65,6 +84,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
      params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
    }
  }
+
  return parameters;
 }

--- a/rclcpp/test/rclcpp/CMakeLists.txt
+++ b/rclcpp/test/rclcpp/CMakeLists.txt
@@ -143,6 +143,13 @@ if(TARGET test_intra_process_manager)
  )
  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
 endif()
+ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp)
+if(TARGET test_intra_process_manager_with_allocators)
+  ament_target_dependencies(test_intra_process_manager_with_allocators
+    "test_msgs"
+  )
+  target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
 if(TARGET test_ring_buffer_implementation)
  ament_target_dependencies(test_ring_buffer_implementation
@@ -558,6 +565,13 @@ if(TARGET test_utilities)
  target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
 endif()

+ament_add_gtest(test_wait_for_message test_wait_for_message.cpp)
+if(TARGET test_wait_for_message)
+  ament_target_dependencies(test_wait_for_message
+    "test_msgs")
+  target_link_libraries(test_wait_for_message ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(test_interface_traits test_interface_traits.cpp
  APPEND_LIBRARY_DIRS "${append_library_dirs}")
 if(TARGET test_interface_traits)
--- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp
+++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp
@@ -41,38 +41,38 @@ struct NumberOfEntities
 std::unique_ptr<NumberOfEntities> get_number_of_default_entities(rclcpp::Node::SharedPtr node)
 {
  auto number_of_entities = std::make_unique<NumberOfEntities>();
-  for (auto & weak_group : node->get_callback_groups()) {
-    auto group = weak_group.lock();
-    EXPECT_NE(nullptr, group);
-    if (!group || !group->can_be_taken_from().load()) {
-      return nullptr;
-    }
-    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;
-      });
-  }
+  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;
 }
--- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp
+++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp
@@ -31,6 +31,8 @@
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"

+#include "rcpputils/filesystem_helper.hpp"
+
 class TestNodeParameters : public ::testing::Test
 {
 public:
@@ -47,6 +49,7 @@ public:
      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
      node->get_node_parameters_interface().get());
    ASSERT_NE(nullptr, node_parameters);
+    test_resources_path /= "test_node_parameters";
  }

  void TearDown()
@@ -57,6 +60,8 @@ public:
 protected:
  std::shared_ptr<rclcpp::Node> node;
  rclcpp::node_interfaces::NodeParameters * node_parameters;
+
+  rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY};
 };

 TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
@@ -199,3 +204,130 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) {
    node_parameters->remove_on_set_parameters_callback(handle.get()),
    std::runtime_error("Callback doesn't exist"));
 }
+
+TEST_F(TestNodeParameters, wildcard_with_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(7u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_one_star").get<std::string>(),
+    "namespace_wild_one_star");
+  EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get<std::string>(), "node_wild_in_ns");
+  EXPECT_EQ(
+    parameter_overrides.at("node_wild_in_ns_another").get<std::string>(),
+    "node_wild_in_ns_another");
+  EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get<std::string>(), "explicit_in_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+}
+
+TEST_F(TestNodeParameters, wildcard_no_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(5u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get<std::string>(), "node_wild_no_ns");
+  EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get<std::string>(), "explicit_no_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+  // "/*" match exactly one token, not expect to get `namespace_wild_one_star`
+  EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u);
+}
+
+TEST_F(TestNodeParameters, params_by_order)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "params_by_order.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(3u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("a_value").get<std::string>(), "last_one_win");
+  EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+  EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+}
+
+TEST_F(TestNodeParameters, complicated_wildcards)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "complicated_wildcards.yaml").string()
+  });
+
+  {
+    // regex matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/d/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(2u, parameter_overrides.size());
+    EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+    EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+  }
+
+  {
+    // regex not matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(0u, parameter_overrides.size());
+  }
+}
--- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp
+++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp
@@ -23,6 +23,7 @@

 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"

@@ -114,12 +115,11 @@ protected:
  {
    auto node = std::make_shared<rclcpp::Node>(name, "ns");

-    for (auto & group_weak_ptr : node->get_callback_groups()) {
-      auto group = group_weak_ptr.lock();
-      if (group) {
+    node->for_each_callback_group(
+      [](rclcpp::CallbackGroup::SharedPtr group)
+      {
        group->can_be_taken_from() = false;
-      }
-    }
+      });
    return node;
  }

@@ -201,15 +201,13 @@ protected:
    const RclWaitSetSizes & expected)
  {
    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -234,15 +232,13 @@ protected:
    const RclWaitSetSizes & insufficient_capacities)
  {
    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -316,26 +312,22 @@ protected:
  {
    auto basic_node = create_node_with_disabled_callback_groups("basic_node");
    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = basic_node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes,
-      &basic_node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    basic_node->for_each_callback_group(
+      [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            basic_node->get_node_base_interface()));
      });
-    auto callback_groups1 = node_with_entity1->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups1.begin(), callback_groups1.end(),
-      [&weak_groups_to_nodes,
-      &node_with_entity1](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_with_entity1->for_each_callback_group(
+      [node_with_entity1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node_with_entity1->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -348,26 +340,23 @@ protected:

    auto basic_node2 = std::make_shared<rclcpp::Node>("basic_node2", "ns");
    WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes;
-    auto callback_groups2 = basic_node2->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups2.begin(), callback_groups2.end(),
-      [&weak_groups_to_uncollected_nodes,
-      &basic_node2](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    basic_node2->for_each_callback_group(
+      [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_uncollected_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            basic_node2->get_node_base_interface()));
      });
-    auto callback_groups3 = node_with_entity2->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups3.begin(), callback_groups3.end(),
-      [&weak_groups_to_uncollected_nodes,
-      &node_with_entity2](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node_with_entity2->for_each_callback_group(
+      [node_with_entity2,
+      &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_uncollected_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node_with_entity2->get_node_base_interface()));
      });
    rclcpp::AnyExecutable failed_result = get_next_entity_func(weak_groups_to_uncollected_nodes);
@@ -388,24 +377,24 @@ protected:
    auto basic_node_base = basic_node->get_node_base_interface();
    auto node_base = node_with_entity->get_node_base_interface();
    WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-    auto callback_groups = basic_node_base->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &basic_node_base](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    rclcpp::node_interfaces::global_for_each_callback_group(
+      basic_node_base.get(),
+      [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            basic_node_base));
      });
-    callback_groups = node_base->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node_base](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    rclcpp::node_interfaces::global_for_each_callback_group(
+      node_base.get(),
+      [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node_base));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -447,14 +436,13 @@ private:
 TEST_F(TestAllocatorMemoryStrategy, construct_destruct) {
  auto basic_node = create_node_with_disabled_callback_groups("basic_node");
  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = basic_node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &basic_node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  basic_node->for_each_callback_group(
+    [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
      weak_groups_to_nodes.insert(
        std::pair<rclcpp::CallbackGroup::WeakPtr,
        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
          basic_node->get_node_base_interface()));
    });
  allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -546,14 +534,13 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) {
 TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) {
  auto node = create_node_with_subscription("subscription_node");
  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  node->for_each_callback_group(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
      weak_groups_to_nodes.insert(
        std::pair<rclcpp::CallbackGroup::WeakPtr,
        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
          node->get_node_base_interface()));
    });
  allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -787,14 +774,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) {
      test_msgs::msg::Empty, decltype(subscription_callback)>(
      "topic", qos, std::move(subscription_callback), subscription_options);

-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -821,14 +807,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
    auto service = node->create_service<test_msgs::srv::Empty>(
      "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group);

-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -843,14 +828,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) {
 TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) {
  auto node = create_node_with_disabled_callback_groups("node");
  WeakCallbackGroupsToNodesMap weak_groups_to_nodes;
-  auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-  std::for_each(
-    callback_groups.begin(), callback_groups.end(),
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+  node->for_each_callback_group(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
      weak_groups_to_nodes.insert(
        std::pair<rclcpp::CallbackGroup::WeakPtr,
        rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-          weak_group_ptr,
+          group_ptr,
          node->get_node_base_interface()));
    });
  // Force client to go out of scope and cleanup after collecting entities.
@@ -886,14 +870,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) {
      rclcpp::CallbackGroupType::MutuallyExclusive);
    auto timer = node->create_wall_timer(
      std::chrono::seconds(10), []() {}, callback_group);
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -913,14 +896,13 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) {
    auto callback_group =
      node->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
--- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp
+++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp
@@ -216,7 +216,10 @@ public:
  const char * topic_name;
 };

-template<typename MessageT>
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
 class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 {
 public:
--- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
+++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
@@ -0,0 +1,276 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+
+#include <chrono>
+#include <list>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "test_msgs/msg/empty.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+
+// For demonstration purposes only, not necessary for allocator_traits
+static uint32_t num_allocs = 0;
+static uint32_t num_deallocs = 0;
+// A very simple custom allocator. Counts calls to allocate and deallocate.
+template<typename T = void>
+struct MyAllocator
+{
+public:
+  using value_type = T;
+  using size_type = std::size_t;
+  using pointer = T *;
+  using const_pointer = const T *;
+  using difference_type = typename std::pointer_traits<pointer>::difference_type;
+
+  MyAllocator() noexcept
+  {
+  }
+
+  ~MyAllocator() noexcept {}
+
+  template<typename U>
+  MyAllocator(const MyAllocator<U> &) noexcept
+  {
+  }
+
+  T * allocate(size_t size, const void * = 0)
+  {
+    if (size == 0) {
+      return nullptr;
+    }
+    num_allocs++;
+    return static_cast<T *>(std::malloc(size * sizeof(T)));
+  }
+
+  void deallocate(T * ptr, size_t size)
+  {
+    (void)size;
+    if (!ptr) {
+      return;
+    }
+    num_deallocs++;
+    std::free(ptr);
+  }
+
+  template<typename U>
+  struct rebind
+  {
+    typedef MyAllocator<U> other;
+  };
+};
+
+template<typename T, typename U>
+constexpr bool operator==(
+  const MyAllocator<T> &,
+  const MyAllocator<U> &) noexcept
+{
+  return true;
+}
+
+template<typename T, typename U>
+constexpr bool operator!=(
+  const MyAllocator<T> &,
+  const MyAllocator<U> &) noexcept
+{
+  return false;
+}
+
+template<
+  typename PublishedMessageAllocatorT,
+  typename PublisherAllocatorT,
+  typename SubscribedMessageAllocatorT,
+  typename SubscriptionAllocatorT,
+  typename MessageMemoryStrategyAllocatorT,
+  typename MemoryStrategyAllocatorT,
+  typename ExpectedExceptionT
+>
+void
+do_custom_allocator_test(
+  PublishedMessageAllocatorT published_message_allocator,
+  PublisherAllocatorT publisher_allocator,
+  SubscribedMessageAllocatorT /* subscribed_message_allocator */,  // intentionally unused
+  SubscriptionAllocatorT subscription_allocator,
+  MessageMemoryStrategyAllocatorT message_memory_strategy,
+  MemoryStrategyAllocatorT memory_strategy_allocator)
+{
+  using PublishedMessageAllocTraits =
+    rclcpp::allocator::AllocRebind<test_msgs::msg::Empty, PublishedMessageAllocatorT>;
+  using PublishedMessageAlloc = typename PublishedMessageAllocTraits::allocator_type;
+  using PublishedMessageDeleter =
+    rclcpp::allocator::Deleter<PublishedMessageAlloc, test_msgs::msg::Empty>;
+
+  using SubscribedMessageAllocTraits =
+    rclcpp::allocator::AllocRebind<test_msgs::msg::Empty, SubscribedMessageAllocatorT>;
+  using SubscribedMessageAlloc = typename SubscribedMessageAllocTraits::allocator_type;
+  using SubscribedMessageDeleter =
+    rclcpp::allocator::Deleter<SubscribedMessageAlloc, test_msgs::msg::Empty>;
+
+  // init and node
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>(
+    "custom_allocator_test",
+    rclcpp::NodeOptions().context(context).use_intra_process_comms(true));
+
+  // publisher
+  auto shared_publisher_allocator = std::make_shared<PublisherAllocatorT>(publisher_allocator);
+  rclcpp::PublisherOptionsWithAllocator<PublisherAllocatorT> publisher_options;
+  publisher_options.allocator = shared_publisher_allocator;
+  auto publisher =
+    node->create_publisher<test_msgs::msg::Empty>("custom_allocator_test", 10, publisher_options);
+
+  // callback for subscription
+  uint32_t counter = 0;
+  std::promise<std::unique_ptr<test_msgs::msg::Empty, SubscribedMessageDeleter>> received_message;
+  auto received_message_future = received_message.get_future();
+  auto callback =
+    [&counter, &received_message](
+    std::unique_ptr<test_msgs::msg::Empty, SubscribedMessageDeleter> msg)
+    {
+      ++counter;
+      received_message.set_value(std::move(msg));
+    };
+
+  // subscription
+  auto shared_subscription_allocator =
+    std::make_shared<SubscriptionAllocatorT>(subscription_allocator);
+  rclcpp::SubscriptionOptionsWithAllocator<SubscriptionAllocatorT> subscription_options;
+  subscription_options.allocator = shared_subscription_allocator;
+  auto shared_message_strategy_allocator =
+    std::make_shared<MessageMemoryStrategyAllocatorT>(message_memory_strategy);
+  auto msg_mem_strat = std::make_shared<
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      test_msgs::msg::Empty,
+      MessageMemoryStrategyAllocatorT
+    >
+    >(shared_message_strategy_allocator);
+  using CallbackMessageT =
+    typename rclcpp::subscription_traits::has_message_type<decltype(callback)>::type;
+  auto subscriber = node->create_subscription<
+    test_msgs::msg::Empty,
+    decltype(callback),
+    SubscriptionAllocatorT,
+    CallbackMessageT,
+    rclcpp::Subscription<CallbackMessageT, SubscriptionAllocatorT>,
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      CallbackMessageT,
+      MessageMemoryStrategyAllocatorT
+    >
+    >(
+    "custom_allocator_test",
+    10,
+    std::forward<decltype(callback)>(callback),
+    subscription_options,
+    msg_mem_strat);
+
+  // executor memory strategy
+  using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+  auto shared_memory_strategy_allocator = std::make_shared<MemoryStrategyAllocatorT>(
+    memory_strategy_allocator);
+  std::shared_ptr<rclcpp::memory_strategy::MemoryStrategy> memory_strategy =
+    std::make_shared<AllocatorMemoryStrategy<MemoryStrategyAllocatorT>>(
+    shared_memory_strategy_allocator);
+
+  // executor
+  rclcpp::ExecutorOptions options;
+  options.memory_strategy = memory_strategy;
+  options.context = context;
+  rclcpp::executors::SingleThreadedExecutor executor(options);
+
+  executor.add_node(node);
+
+  // rebind message_allocator to ensure correct type
+  PublishedMessageDeleter message_deleter;
+  PublishedMessageAlloc rebound_message_allocator = published_message_allocator;
+  rclcpp::allocator::set_allocator_for_deleter(&message_deleter, &rebound_message_allocator);
+
+  // allocate a message
+  auto ptr = PublishedMessageAllocTraits::allocate(rebound_message_allocator, 1);
+  PublishedMessageAllocTraits::construct(rebound_message_allocator, ptr);
+  std::unique_ptr<test_msgs::msg::Empty, PublishedMessageDeleter> msg(ptr, message_deleter);
+
+  // publisher and receive
+  if constexpr (std::is_same_v<ExpectedExceptionT, void>) {
+    // no exception expected
+    EXPECT_NO_THROW(
+    {
+      publisher->publish(std::move(msg));
+      executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10));
+    });
+    EXPECT_EQ(ptr, received_message_future.get().get());
+    EXPECT_EQ(1u, counter);
+  } else {
+    // exception expected
+    EXPECT_THROW(
+    {
+      publisher->publish(std::move(msg));
+      executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10));
+    }, ExpectedExceptionT);
+  }
+}
+
+/*
+   This tests the case where a custom allocator is used correctly, i.e. the same
+   custom allocator on both sides.
+ */
+TEST(TestIntraProcessManagerWithAllocators, custom_allocator) {
+  using PublishedMessageAllocatorT = std::allocator<void>;
+  using PublisherAllocatorT = std::allocator<void>;
+  using SubscribedMessageAllocatorT = std::allocator<void>;
+  using SubscriptionAllocatorT = std::allocator<void>;
+  using MessageMemoryStrategyAllocatorT = std::allocator<void>;
+  using MemoryStrategyAllocatorT = std::allocator<void>;
+  auto allocator = std::allocator<void>();
+  do_custom_allocator_test<
+    PublishedMessageAllocatorT,
+    PublisherAllocatorT,
+    SubscribedMessageAllocatorT,
+    SubscriptionAllocatorT,
+    MessageMemoryStrategyAllocatorT,
+    MemoryStrategyAllocatorT,
+    void  // no exception expected
+  >(allocator, allocator, allocator, allocator, allocator, allocator);
+}
+
+/*
+   This tests the case where a custom allocator is used incorrectly, i.e. different
+   custom allocators on both sides.
+ */
+TEST(TestIntraProcessManagerWithAllocators, custom_allocator_wrong) {
+  // explicitly use a different allocator here to provoke a failure
+  using PublishedMessageAllocatorT = std::allocator<void>;
+  using PublisherAllocatorT = std::allocator<void>;
+  using SubscribedMessageAllocatorT = MyAllocator<void>;
+  using SubscriptionAllocatorT = MyAllocator<void>;
+  using MessageMemoryStrategyAllocatorT = MyAllocator<void>;
+  using MemoryStrategyAllocatorT = std::allocator<void>;
+  auto allocator = std::allocator<void>();
+  auto my_allocator = MyAllocator<void>();
+  do_custom_allocator_test<
+    PublishedMessageAllocatorT,
+    PublisherAllocatorT,
+    SubscribedMessageAllocatorT,
+    SubscriptionAllocatorT,
+    MessageMemoryStrategyAllocatorT,
+    MemoryStrategyAllocatorT,
+    std::runtime_error  // expected exception
+  >(allocator, allocator, my_allocator, my_allocator, my_allocator, allocator);
+}
--- a/rclcpp/test/rclcpp/test_logging.cpp
+++ b/rclcpp/test/rclcpp/test_logging.cpp
@@ -229,6 +229,12 @@ TEST_F(TestLoggingMacros, test_throttle) {
  }
 }

+TEST_F(TestLoggingMacros, test_parameter_expression) {
+  RCLCPP_DEBUG_STREAM(*&g_logger, "message");
+  EXPECT_EQ(1u, g_log_calls);
+  EXPECT_EQ("message", g_last_log_event.message);
+}
+
 bool log_function(rclcpp::Logger logger)
 {
  RCLCPP_INFO(logger, "successful log");
--- a/rclcpp/test/rclcpp/test_memory_strategy.cpp
+++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp
@@ -21,6 +21,7 @@

 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
 #include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"

@@ -84,14 +85,13 @@ TEST_F(TestMemoryStrategy, get_subscription_by_handle) {
  rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -131,14 +131,13 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) {
  rclcpp::ServiceBase::SharedPtr found_service = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -184,14 +183,13 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) {
  rclcpp::ClientBase::SharedPtr found_client = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -232,14 +230,13 @@ TEST_F(TestMemoryStrategy, get_timer_by_handle) {
  rclcpp::TimerBase::SharedPtr found_timer = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -281,14 +278,14 @@ TEST_F(TestMemoryStrategy, get_node_by_group) {
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
    auto node_handle = node->get_node_base_interface();
-    auto callback_groups = node_handle->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node_handle](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    rclcpp::node_interfaces::global_for_each_callback_group(
+      node_handle.get(),
+      [node_handle, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node_handle));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -325,14 +322,13 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) {
  rclcpp::CallbackGroup::SharedPtr callback_group = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -379,14 +375,13 @@ TEST_F(TestMemoryStrategy, get_group_by_service) {
  rclcpp::ServiceBase::SharedPtr service = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -424,14 +419,13 @@ TEST_F(TestMemoryStrategy, get_group_by_client) {
  rclcpp::ClientBase::SharedPtr client = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -464,14 +458,13 @@ TEST_F(TestMemoryStrategy, get_group_by_timer) {
  rclcpp::TimerBase::SharedPtr timer = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
@@ -504,14 +497,13 @@ TEST_F(TestMemoryStrategy, get_group_by_waitable) {
  rclcpp::Waitable::SharedPtr waitable = nullptr;
  {
    auto node = std::make_shared<rclcpp::Node>("node", "ns");
-    auto callback_groups = node->get_node_base_interface()->get_callback_groups();
-    std::for_each(
-      callback_groups.begin(), callback_groups.end(),
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_group_ptr) {
+    node->for_each_callback_group(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
+      {
        weak_groups_to_nodes.insert(
          std::pair<rclcpp::CallbackGroup::WeakPtr,
          rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
-            weak_group_ptr,
+            group_ptr,
            node->get_node_base_interface()));
      });
    memory_strategy()->collect_entities(weak_groups_to_nodes);
--- a/rclcpp/test/rclcpp/test_node.cpp
+++ b/rclcpp/test/rclcpp/test_node.cpp
@@ -99,6 +99,7 @@ TEST_F(TestNode, get_name_and_namespace) {
    auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/ns", node->get_namespace());
+    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
  }
  {
@@ -113,30 +114,35 @@ TEST_F(TestNode, get_name_and_namespace) {
    auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/ns", node->get_namespace());
+    EXPECT_STREQ("/ns", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name());
  }
  {
    auto node = std::make_shared<rclcpp::Node>("my_node");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/", node->get_namespace());
+    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
  }
  {
    auto node = std::make_shared<rclcpp::Node>("my_node", "");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/", node->get_namespace());
+    EXPECT_STREQ("/", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/my_node", node->get_fully_qualified_name());
  }
  {
    auto node = std::make_shared<rclcpp::Node>("my_node", "/my/ns");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/my/ns", node->get_namespace());
+    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
  }
  {
    auto node = std::make_shared<rclcpp::Node>("my_node", "my/ns");
    EXPECT_STREQ("my_node", node->get_name());
    EXPECT_STREQ("/my/ns", node->get_namespace());
+    EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str());
    EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name());
  }
  {
@@ -275,6 +281,13 @@ TEST_F(TestNode, subnode_construction_and_destruction) {
      auto subnode = node->create_sub_node("~sub_ns");
    }, rclcpp::exceptions::InvalidNamespaceError);
  }
+  {
+    ASSERT_THROW(
+    {
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("");
+    }, rclcpp::exceptions::NameValidationError);
+  }
 }

 TEST_F(TestNode, get_logger) {
@@ -333,9 +346,14 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
    rcl_interfaces::msg::ParameterDescriptor descriptor;
    descriptor.dynamic_typing = true;
    // no default, no initial
+    const std::string parameter_name = "parameter"_unq;
    rclcpp::ParameterValue value = node->declare_parameter(
-      "parameter"_unq, rclcpp::ParameterValue{}, descriptor);
+      parameter_name, rclcpp::ParameterValue{}, descriptor);
    EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
+    // Does not throw if unset before access
+    EXPECT_EQ(
+      rclcpp::PARAMETER_NOT_SET,
+      node->get_parameter(parameter_name).get_parameter_value().get_type());
  }
  {
    // int default, no initial
@@ -2637,13 +2655,33 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {

 TEST_F(TestNode, callback_groups) {
  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  size_t num_callback_groups_in_basic_node = node->get_callback_groups().size();
+  size_t num_callback_groups_in_basic_node = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups_in_basic_node](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups_in_basic_node++;
+    });

  auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_EQ(1u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+  size_t num_callback_groups = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups++;
+    });
+  EXPECT_EQ(1u + num_callback_groups_in_basic_node, num_callback_groups);

  auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  EXPECT_EQ(2u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+  size_t num_callback_groups2 = 0;
+  node->for_each_callback_group(
+    [&num_callback_groups2](rclcpp::CallbackGroup::SharedPtr group)
+    {
+      (void)group;
+      num_callback_groups2++;
+    });
+  EXPECT_EQ(2u + num_callback_groups_in_basic_node, num_callback_groups2);
 }

 // This is tested more thoroughly in node_interfaces/test_node_graph
@@ -2761,9 +2799,20 @@ TEST_F(TestNode, static_and_dynamic_typing) {
    EXPECT_EQ("hello!", param);
  }
  {
+    auto param = node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type());
+    // Throws if not set before access
    EXPECT_THROW(
-      node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER),
-      rclcpp::exceptions::NoParameterOverrideProvided);
+      node->get_parameter("integer_override_not_given"),
+      rclcpp::exceptions::ParameterUninitializedException);
+  }
+  {
+    auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type());
+    auto result = node->set_parameter(rclcpp::Parameter{"integer_set_after_declare", 44});
+    ASSERT_TRUE(result.successful) << result.reason;
+    auto get_param = node->get_parameter("integer_set_after_declare");
+    EXPECT_EQ(44, get_param.as_int());
  }
  {
    EXPECT_THROW(
--- a/rclcpp/test/rclcpp/test_parameter_map.cpp
+++ b/rclcpp/test/rclcpp/test_parameter_map.cpp
@@ -19,6 +19,7 @@

 #include <cstdio>
 #include <string>
+#include <unordered_map>
 #include <vector>

 #include "rclcpp/parameter_map.hpp"
@@ -353,3 +354,132 @@ TEST(Test_parameter_map_from, string_array_param_value)
  c_params->params[0].parameter_values[0].string_array_value = NULL;
  rcl_yaml_node_struct_fini(c_params);
 }
+
+TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"string_param"});
+
+  std::string hello_world = "hello world";
+  char * c_hello_world = new char[hello_world.length() + 1];
+  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+  c_params->params[0].parameter_values[0].string_value = c_hello_world;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo");
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("string_param", params.at(0).get_name().c_str());
+  EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value<std::string>().c_str());
+
+  c_params->params[0].parameter_values[0].string_value = NULL;
+  delete[] c_hello_world;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn)
+{
+  std::vector<std::string> node_names_keys = {
+    "/**",              // index: 0
+    "/*",               // index: 1
+    "/**/node",         // index: 2
+    "/*/node",          // index: 3
+    "/ns/node"          // index: 4
+  };
+
+  rcl_params_t * c_params = make_params(node_names_keys);
+
+  std::vector<char *> param_values;
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    make_node_params(c_params, i, {"string_param"});
+    std::string hello_world = "hello world" + std::to_string(i);
+    char * c_hello_world = new char[hello_world.length() + 1];
+    std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+    c_params->params[i].parameter_values[0].string_value = c_hello_world;
+    param_values.push_back(c_hello_world);
+  }
+
+  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
+    {"/ns/foo/another_node", {0}},
+    {"/another", {0, 1}},
+    {"/node", {0, 1, 2}},
+    {"/another_ns/node", {0, 2, 3}},
+    {"/ns/node", {0, 2, 3, 4}},
+  };
+
+  for (auto & kv : node_fqn_expected) {
+    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
+    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
+
+    EXPECT_EQ(kv.second.size(), params.size());
+    for (size_t i = 0; i < params.size(); ++i) {
+      std::string param_value = "hello world" + std::to_string(kv.second[i]);
+      EXPECT_STREQ("string_param", params.at(i).get_name().c_str());
+      EXPECT_STREQ(param_value.c_str(), params.at(i).get_value<std::string>().c_str());
+    }
+  }
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = NULL;
+  }
+  for (auto c_hello_world : param_values) {
+    delete[] c_hello_world;
+  }
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn)
+{
+  std::vector<std::string> node_names_keys = {
+    "/**",              // index: 0
+    "/*",               // index: 1
+    "/**/node",         // index: 2
+    "/*/node",          // index: 3
+    "/ns/**",           // index: 4
+    "/ns/*",            // index: 5
+    "/ns/**/node",      // index: 6
+    "/ns/*/node",       // index: 7
+    "/ns/**/a/*/node",  // index: 8
+    "/ns/node"          // index: 9
+  };
+
+  rcl_params_t * c_params = make_params(node_names_keys);
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    std::string param_name = "string_param" + std::to_string(i);
+    make_node_params(c_params, i, {param_name});
+  }
+
+  std::string hello_world = "hello world";
+  char * c_hello_world = new char[hello_world.length() + 1];
+  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = c_hello_world;
+  }
+
+  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
+    {"/ns/node", {0, 2, 3, 4, 5, 6, 9}},
+    {"/node", {0, 1, 2}},
+    {"/ns/foo/node", {0, 2, 4, 6, 7}},
+    {"/ns/foo/a/node", {0, 2, 4, 6}},
+    {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}},
+    {"/ns/a/bar/node", {0, 2, 4, 6, 8}},
+    {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}},
+  };
+
+  for (auto & kv : node_fqn_expected) {
+    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
+    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
+    EXPECT_EQ(kv.second.size(), params.size());
+    for (size_t i = 0; i < params.size(); ++i) {
+      std::string param_name = "string_param" + std::to_string(kv.second[i]);
+      EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str());
+      EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value<std::string>().c_str());
+    }
+  }
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = NULL;
+  }
+  delete[] c_hello_world;
+  rcl_yaml_node_struct_fini(c_params);
+}
--- a/rclcpp/test/rclcpp/test_wait_for_message.cpp
+++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp
@@ -0,0 +1,74 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/wait_for_message.hpp"
+
+#include "test_msgs/msg/strings.hpp"
+#include "test_msgs/message_fixtures.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestUtilities, wait_for_message) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node");
+
+  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]);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestUtilities, wait_for_message_indefinitely) {
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node2");
+
+  using MsgT = test_msgs::msg::Strings;
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic" /*, -1 */);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  rclcpp::shutdown();
+
+  ASSERT_FALSE(received);
+}
--- a/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml
+++ b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml
@@ -0,0 +1,5 @@
+/**/foo/*/bar:
+  node2:
+    ros__parameters:
+      foo: "foo"
+      bar: "bar"
--- a/rclcpp/test/resources/test_node_parameters/params_by_order.yaml
+++ b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml
@@ -0,0 +1,16 @@
+/**:
+  node2:
+    ros__parameters:
+      a_value: "first"
+      foo: "foo"
+
+/ns:
+  node2:
+    ros__parameters:
+      a_value: "second"
+      bar: "bar"
+
+/*:
+  node2:
+    ros__parameters:
+      a_value: "last_one_win"
--- a/rclcpp/test/resources/test_node_parameters/wildcards.yaml
+++ b/rclcpp/test/resources/test_node_parameters/wildcards.yaml
@@ -0,0 +1,57 @@
+/**:
+  ros__parameters:
+    full_wild: "full_wild"
+
+/**:
+  node2:
+    ros__parameters:
+      namespace_wild: "namespace_wild"
+
+/**/node2:
+  ros__parameters:
+    namespace_wild_another: "namespace_wild_another"
+
+/*:
+  node2:
+    ros__parameters:
+      namespace_wild_one_star: "namespace_wild_one_star"
+
+ns:
+  "*":
+    ros__parameters:
+      node_wild_in_ns: "node_wild_in_ns"
+
+/ns/*:
+  ros__parameters:
+    node_wild_in_ns_another: "node_wild_in_ns_another"
+
+ns:
+  node2:
+    ros__parameters:
+      explicit_in_ns: "explicit_in_ns"
+
+"*":
+  ros__parameters:
+    node_wild_no_ns: "node_wild_no_ns"
+
+node2:
+  ros__parameters:
+    explicit_no_ns: "explicit_no_ns"
+
+ns:
+  nodeX:
+    ros__parameters:
+      should_not_appear: "incorrect_node_name"
+
+/**/nodeX:
+  ros__parameters:
+    should_not_appear: "incorrect_node_name"
+
+nsX:
+  node2:
+    ros__parameters:
+      should_not_appear: "incorrect_namespace"
+
+/nsX/*:
+  ros__parameters:
+    should_not_appear: "incorrect_namespace"
--- a/rclcpp_action/CHANGELOG.rst
+++ b/rclcpp_action/CHANGELOG.rst
@@ -3,6 +3,29 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^


+9.2.2 (2022-12-06)
+------------------
+
+9.2.1 (2022-04-28)
+------------------
+
+9.2.0 (2021-09-17)
+------------------
+* Fixed a bug where it would occasionally miss a goal result, which was caused by race condition (`#1677 <https://github.com/ros2/rclcpp/issues/1677>`_) (`#1683 <https://github.com/ros2/rclcpp/issues/1683>`_)
+  * Co-authored-by: Kaven Yau <kavenyau@foxmail.com>
+* Contributors: Tomoya Fujita
+
+9.1.0 (2021-05-20)
+------------------
+* Bump the benchmark timeout for benchmark_action_client (`#1672 <https://github.com/ros2/rclcpp/issues/1672>`_)
+* Contributors: Scott K Logan
+
+9.0.3 (2021-05-10)
+------------------
+* Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (`#1641 <https://github.com/ros2/rclcpp/issues/1641>`_) (`#1653 <https://github.com/ros2/rclcpp/issues/1653>`_)
+* Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (`#1635 <https://github.com/ros2/rclcpp/issues/1635>`_) (`#1646 <https://github.com/ros2/rclcpp/issues/1646>`_)
+* Contributors: Jacob Perron, Kaven Yau, William Woodall
+
 9.0.2 (2021-04-14)
 ------------------

--- a/rclcpp_action/include/rclcpp_action/server.hpp
+++ b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -356,15 +356,26 @@ protected:
  CancelResponse
  call_handle_cancel_callback(const GoalUUID & uuid) override
  {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      auto element = goal_handles_.find(uuid);
+      if (element != goal_handles_.end()) {
+        goal_handle = element->second.lock();
+      }
+    }
+
    CancelResponse resp = CancelResponse::REJECT;
-    auto element = goal_handles_.find(uuid);
-    if (element != goal_handles_.end()) {
-      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
-      if (goal_handle) {
-        resp = handle_cancel_(goal_handle);
-        if (CancelResponse::ACCEPT == resp) {
+    if (goal_handle) {
+      resp = handle_cancel_(goal_handle);
+      if (CancelResponse::ACCEPT == resp) {
+        try {
          goal_handle->_cancel_goal();
+        } catch (const rclcpp::exceptions::RCLError & ex) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
+          return CancelResponse::REJECT;
        }
      }
    }
--- a/rclcpp_action/package.xml
+++ b/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>9.0.2</version>
+  <version>9.2.2</version>
  <description>Adds action APIs for C++.</description>
  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
--- a/rclcpp_action/src/server.cpp
+++ b/rclcpp_action/src/server.cpp
@@ -500,9 +500,13 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
    result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
  } else {
    // Goal exists, check if a result is already available
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
    auto iter = pimpl_->goal_results_.find(uuid);
    if (iter != pimpl_->goal_results_.end()) {
      result_response = iter->second;
+    } else {
+      // Store the request so it can be responded to later
+      pimpl_->result_requests_[uuid].push_back(request_header);
    }
  }

@@ -514,10 +518,6 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
    if (RCL_RET_OK != rcl_ret) {
      rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
    }
-  } else {
-    // Store the request so it can be responded to later
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
-    pimpl_->result_requests_[uuid].push_back(request_header);
  }
  data.reset();
 }
@@ -627,19 +627,30 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
  }

  {
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+    /**
+    * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and
+    * action_server_reentrant_mutex_ locked in other block scopes. Unless using
+    * std::scoped_lock, locking order must be consistent with the current.
+    *
+    * Current locking order:
+    *
+    *   1. unordered_map_mutex_
+    *   2. action_server_reentrant_mutex_
+    *
+    */
+    std::lock_guard<std::recursive_mutex> unordered_map_lock(pimpl_->unordered_map_mutex_);
    pimpl_->goal_results_[uuid] = result_msg;
-  }

-  // if there are clients who already asked for the result, send it to them
-  auto iter = pimpl_->result_requests_.find(uuid);
-  if (iter != pimpl_->result_requests_.end()) {
-    for (auto & request_header : iter->second) {
+    // if there are clients who already asked for the result, send it to them
+    auto iter = pimpl_->result_requests_.find(uuid);
+    if (iter != pimpl_->result_requests_.end()) {
      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-      rcl_ret_t ret = rcl_action_send_result_response(
-        pimpl_->action_server_.get(), &request_header, result_msg.get());
-      if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+      for (auto & request_header : iter->second) {
+        rcl_ret_t ret = rcl_action_send_result_response(
+          pimpl_->action_server_.get(), &request_header, result_msg.get());
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
      }
    }
  }
--- a/rclcpp_action/test/benchmark/CMakeLists.txt
+++ b/rclcpp_action/test/benchmark/CMakeLists.txt
@@ -7,7 +7,7 @@ find_package(performance_test_fixture REQUIRED)
 add_performance_test(
  benchmark_action_client
  benchmark_action_client.cpp
-  TIMEOUT 120)
+  TIMEOUT 240)
 if(TARGET benchmark_action_client)
  target_link_libraries(benchmark_action_client ${PROJECT_NAME})
  ament_target_dependencies(benchmark_action_client rclcpp test_msgs)
--- a/rclcpp_action/test/test_server.cpp
+++ b/rclcpp_action/test/test_server.cpp
@@ -1306,3 +1306,15 @@ TEST_F(TestDeadlockServer, deadlock_while_canceled)
  send_goal_request(node_, uuid2_);  // deadlock here
  t.join();
 }
+
+TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled)
+{
+  send_goal_request(node_, uuid1_);
+  std::thread t(&TestDeadlockServer::GoalSucceeded, this);
+  rclcpp::sleep_for(std::chrono::milliseconds(50));
+  auto response_ptr = send_cancel_request(node_, uuid1_);
+
+  // current goal handle is not cancelable, so it returns ERROR_REJECTED
+  EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code);
+  t.join();
+}
--- a/rclcpp_components/CHANGELOG.rst
+++ b/rclcpp_components/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

+9.2.2 (2022-12-06)
+------------------
+
+9.2.1 (2022-04-28)
+------------------
+
+9.2.0 (2021-09-17)
+------------------
+
+9.1.0 (2021-05-20)
+------------------
+
+9.0.3 (2021-05-10)
+------------------
+
 9.0.2 (2021-04-14)
 ------------------

--- a/rclcpp_components/package.xml
+++ b/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>9.0.2</version>
+  <version>9.2.2</version>
  <description>Package containing tools for dynamically loadable components</description>
  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
--- a/rclcpp_lifecycle/CHANGELOG.rst
+++ b/rclcpp_lifecycle/CHANGELOG.rst
@@ -3,6 +3,25 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^


+9.2.2 (2022-12-06)
+------------------
+
+9.2.1 (2022-04-28)
+------------------
+
+9.2.0 (2021-09-17)
+------------------
+* Added thread safe for_each_callback_group method (`#1741 <https://github.com/ros2/rclcpp/issues/1741>`_)
+* Contributors: Aditya Pande
+
+9.1.0 (2021-05-20)
+------------------
+* Fix destruction order in lifecycle benchmark (`#1676 <https://github.com/ros2/rclcpp/issues/1676>`_)
+* Contributors: Scott K Logan
+
+9.0.3 (2021-05-10)
+------------------
+
 9.0.2 (2021-04-14)
 ------------------

--- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
+++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
@@ -200,6 +200,18 @@ public:
  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
  get_callback_groups() const;

+  /// Iterate over the callback groups in the node, calling the given function on each valid one.
+  /**
+   * This method is called in a thread-safe way, and also makes sure to only call the given
+   * function on those items that are still valid.
+   *
+   * \param[in] func The callback function to call on each valid callback group.
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  void
+  for_each_callback_group(
+    const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func);
+
  /// Create and return a Publisher.
  /**
   * \param[in] topic_name The topic for this publisher to publish on.
--- a/rclcpp_lifecycle/package.xml
+++ b/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>9.0.2</version>
+  <version>9.2.2</version>
  <description>Package containing a prototype for lifecycle implementation</description>
  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
--- a/rclcpp_lifecycle/src/lifecycle_node.cpp
+++ b/rclcpp_lifecycle/src/lifecycle_node.cpp
@@ -651,4 +651,11 @@ LifecycleNode::add_timer_handle(std::shared_ptr<rclcpp::TimerBase> timer)
  impl_->add_timer_handle(timer);
 }

+void
+LifecycleNode::for_each_callback_group(
+  const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func)
+{
+  rclcpp::node_interfaces::global_for_each_callback_group(node_base_.get(), func);
+}
+
 }  // namespace rclcpp_lifecycle
--- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp
+++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp
@@ -212,8 +212,9 @@ public:
    performance_test_fixture::PerformanceTest::TearDown(state);
    executor->cancel();
    spinner_.join();
-    lifecycle_node.reset();
+    executor.reset();
    lifecycle_client.reset();
+    lifecycle_node.reset();
    rclcpp::shutdown();
  }

--- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp
+++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp
@@ -717,16 +717,27 @@ TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {

 TEST_F(TestDefaultStateMachine, test_callback_groups) {
  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
-  auto groups = test_node->get_callback_groups();
-  EXPECT_EQ(groups.size(), 1u);
+  size_t num_groups = 0;
+  test_node->for_each_callback_group(
+    [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      (void)group_ptr;
+      num_groups++;
+    });
+  EXPECT_EQ(num_groups, 1u);

  auto group = test_node->create_callback_group(
    rclcpp::CallbackGroupType::MutuallyExclusive, true);
  EXPECT_NE(nullptr, group);

-  groups = test_node->get_callback_groups();
-  EXPECT_EQ(groups.size(), 2u);
-  EXPECT_EQ(groups[1].lock().get(), group.get());
+  num_groups = 0;
+  test_node->for_each_callback_group(
+    [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      (void)group_ptr;
+      num_groups++;
+    });
+  EXPECT_EQ(num_groups, 2u);
 }

 TEST_F(TestDefaultStateMachine, wait_for_graph_change)
Author	SHA1	Message	Date
Scott K Logan	7aff0ffc56	9.2.2	2022-12-06 17:59:30 -08:00
mergify[bot]	0b606918d0	Fix returning invalid namespace if sub_namespace is empty (#1658 ) (#1810 ) (cherry picked from commit `3cddb4edab`) Signed-off-by: Markus Hofstaetter <markus.hofstaetter@ait.ac.at> Co-authored-by: M. Hofstätter <markus.hofstaetter@gmx.net>	2022-12-06 17:53:44 -08:00
mergify[bot]	81d6f897a2	use regex for wildcard matching (backport #1839 ) (#1987 ) * use regex for wildcard matching (#1839) * use regex for wildcard matching Co-authored-by: Aaron Lipinski <aaron.lipinski@roboticsplus.co.nz> Signed-off-by: Chen Lihui <lihui.chen@sony.com> * use map to process the content of parameter file by order Signed-off-by: Chen Lihui <lihui.chen@sony.com> * add more test cases Signed-off-by: Chen Lihui <lihui.chen@sony.com> * try to not decrease the performance and make the param win last Signed-off-by: Chen Lihui <lihui.chen@sony.com> * update node name Signed-off-by: Chen Lihui <lihui.chen@sony.com> * update document comment Signed-off-by: Chen Lihui <lihui.chen@sony.com> * add more test for parameter_map_from Signed-off-by: Chen Lihui <lihui.chen@sony.com> Co-authored-by: Aaron Lipinski <aaron.lipinski@roboticsplus.co.nz> (cherry picked from commit `6dd3a0377b`) * not to break ABI Signed-off-by: Chen Lihui <lihui.chen@sony.com> Signed-off-by: Chen Lihui <lihui.chen@sony.com> Co-authored-by: Chen Lihui <lihui.chen@sony.com>	2022-09-09 11:55:20 -07:00
mergify[bot]	206e0fd4fe	Add statistics for handle_loaned_message (backport #1927 ) (#1933 ) * Add statistics for handle_loaned_message (#1927) Signed-off-by: Barry Xu <barry.xu@sony.com> (cherry picked from commit `5c688303b3`) * Fix merge conflicts (#1938) Signed-off-by: Barry Xu <barry.xu@sony.com>	2022-06-09 12:12:57 -07:00
Chris Lalancette	4fc379196d	9.2.1	2022-04-28 15:13:57 +00:00
Chris Lalancette	2f0db6c802	Changelog. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>	2022-04-28 15:13:51 +00:00
mergify[bot]	48068130ed	Add test-dep ament_cmake_google_benchmark (#1904 ) (#1909 ) Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz> (cherry picked from commit `eac0063176`) Co-authored-by: Gaël Écorchard <galou_breizh@yahoo.fr>	2022-04-25 13:33:49 -07:00
mergify[bot]	54c2a8ac5b	Use parantheses around logging macro parameter (#1820 ) (#1822 ) * Use parantheses around logging macro parameter This allows the macro to expand "correctly", i.e. macro argument expression is fully evaluated before use. Signed-off-by: Abrar Rahman Protyasha <aprotyas@u.rochester.edu> * Remove redundant parantheses around macro param `decltype(X)` already provides sufficient "scoping" to the macro parameter `X`. Signed-off-by: Abrar Rahman Protyasha <aprotyas@u.rochester.edu> * Add test case for expressions as logging param Signed-off-by: Abrar Rahman Protyasha <aprotyas@u.rochester.edu> (cherry picked from commit `f7bb88fc8f`) Co-authored-by: Abrar Rahman Protyasha <aprotyas@u.rochester.edu>	2022-02-04 09:09:32 -05:00
George Stavrinos	468cbab1aa	Galactic README now points to galactic docs (#1791 ) Signed-off-by: George Stavrinos <gstavrinos@protonmail.com>	2021-10-01 08:59:01 -04:00
William Woodall	283925677b	9.2.0	2021-09-17 10:07:00 -07:00
William Woodall	340309d05c	changelogs Signed-off-by: William Woodall <william@osrfoundation.org>	2021-09-17 10:05:16 -07:00
Aditya Pande	6adab6eab6	Galactic: for_each_callback_group backport (#1741 ) Added thread safe for_each_callback_group method Signed-off-by: Aditya Pande <aditya050995@gmail.com>	2021-08-19 13:41:19 -07:00
Karsten Knese	910cf32489	wait for message (#1705 ) (#1740 ) * wait for message Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * move to own header file Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * linters Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * add gc for shutdown interrupt Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * mention behavior when shutdown is called Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com> * check gc Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>	2021-08-03 12:15:49 -07:00
mergify[bot]	07f6b642ca	fix documentation bug (#1719 ) (#1720 ) Signed-off-by: William Woodall <william@osrfoundation.org> (cherry picked from commit `86c079de31`) Co-authored-by: William Woodall <william@osrfoundation.org>	2021-07-19 12:06:51 -07:00
Tomoya Fujita	d0fa844a09	Fix occasionally missing goal result caused by race condition (#1677 ) (#1683 ) * Fix occasionally missing goal result caused by race condition Signed-off-by: Kaven Yau <kavenyau@foxmail.com> * Take action_server_reentrant_mutex_ out of the sending result loop Signed-off-by: Kaven Yau <kavenyau@foxmail.com> * add note for explaining the current locking order in server.cpp Signed-off-by: Kaven Yau <kavenyau@foxmail.com> Co-authored-by: Kaven Yau <kavenyau@foxmail.com>	2021-05-25 14:41:41 +09:00
Ivan Santiago Paunovic	260e62d5d6	9.1.0	2021-05-20 21:36:28 +00:00
Ivan Santiago Paunovic	da6c0e7090	Changelogs Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>	2021-05-20 21:32:34 +00:00
Jacob Perron	5a09a4655f	Declare parameters uninitialized (#1673 ) (#1681 ) * Declare parameters uninitialized Fixes #1649 Allow declaring parameters without an initial value or override. This was possible prior to Galactic, but was made impossible since we started enforcing the types of parameters in Galactic. Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Remove assertion Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Throw NoParameterOverrideProvided exception if accessed before initialized Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Add test getting static parameter after it is set Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Do not throw on access of uninitialized dynamically typed parameter Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Rename exception type Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Remove unused exception type Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Uncrustify Signed-off-by: Jacob Perron <jacob@openrobotics.org>	2021-05-20 14:11:14 -07:00
mergify[bot]	e9313c3dc5	Fix destruction order in lifecycle benchmark (#1676 ) (cherry picked from commit `56f68f9c44`) Signed-off-by: Scott K Logan <logans@cottsay.net> Co-authored-by: Scott K Logan <logans@cottsay.net>	2021-05-14 21:49:19 -07:00
mergify[bot]	c02a6a3cd3	Bump the benchmark timeout for benchmark_action_client (#1672 ) Pausing and resuming the measurement inside the timing loop can cause the initial run duration calculation to underestimate how long the benchmark is taking to run, which results in the recorded run taking a lot longer than it should. This is a known issue in libbenchmark. This test is affected by that behavior, and typically takes a bit longer than the others. The easiest thing to do right now is to just bump the timeout. My tests show that 180 seconds is typically sufficient for this test, so 240 should be a safe point to conclude that the test is malfunctioning. (cherry picked from commit `f245b4cc81`) Signed-off-by: Scott K Logan <logans@cottsay.net> Co-authored-by: Scott K Logan <logans@cottsay.net>	2021-05-13 14:09:59 -07:00
mergify[bot]	2d208c5df3	[service] Don't use a weak_ptr to avoid leaking (#1668 ) (#1670 ) Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com> (cherry picked from commit `d488535f36`) Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>	2021-05-13 15:57:50 -03:00
Chris Lalancette	42fb17ff95	9.0.3	2021-05-10 13:00:12 +00:00
Chris Lalancette	2f2232b723	Changelog. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>	2021-05-10 12:59:55 +00:00
mergify[bot]	2616dfaef9	Use OnShutdown callback handle instead of OnShutdown callback (#1639 ) (#1650 ) 1. Add remove_on_shutdown_callback() in rclcpp::Context Signed-off-by: Barry Xu <barry.xu@sony.com> 2. Add add_on_shutdown_callback(), which returns a handle that can be removed by remove_on_shutdown_callback(). Signed-off-by: Barry Xu <barry.xu@sony.com> (cherry picked from commit `6806cdf825`) Co-authored-by: Barry Xu <barry.xu@sony.com>	2021-05-03 09:58:59 -03:00
mergify[bot]	33de648095	Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (#1641 ) (#1653 ) Signed-off-by: Kaven Yau <kavenyau@foxmail.com> (cherry picked from commit `d051b8aa20`) Co-authored-by: Kaven Yau <kavenyau@foxmail.com>	2021-04-30 19:30:45 -03:00
mergify[bot]	82e4e72a2e	Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (#1635 ) (#1646 ) * Fix deadlock issue that caused by other mutexes locked in CancelCallback Signed-off-by: Kaven Yau <love29881460@qq.com> * Add unit test for rclcpp action server deadlock Signed-off-by: Kaven Yau <love29881460@qq.com> * Update rclcpp_action/test/test_server.cpp Co-authored-by: William Woodall <william+github@osrfoundation.org> Co-authored-by: Kaven Yau <love29881460@qq.com> Co-authored-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: William Woodall <william+github@osrfoundation.org> (cherry picked from commit `fba080cf34`) Co-authored-by: Kaven Yau <kavenyau@foxmail.com>	2021-04-30 15:04:09 -03:00
mergify[bot]	7596ed4db0	use dynamic_pointer_cast to detect allocator mismatch in intra process manager (#1643 ) (#1644 ) * use dynamic_pointer_cast to detect allocator mismatch in intra process manager Signed-off-by: William Woodall <william@osrfoundation.org> * add test case for mismatched allocators Signed-off-by: William Woodall <william@osrfoundation.org> * forward template arguments to avoid mismatched types in intra process manager Signed-off-by: William Woodall <william@osrfoundation.org> * style fixes Signed-off-by: William Woodall <william@osrfoundation.org> * refactor to test message address and count, more DRY Signed-off-by: William Woodall <william@osrfoundation.org> * update copyright Signed-off-by: William Woodall <william@osrfoundation.org> * fix typo Signed-off-by: William Woodall <william@osrfoundation.org> Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com> Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com> (cherry picked from commit `79c2dd8e8b`) Co-authored-by: William Woodall <william@osrfoundation.org>	2021-04-29 12:22:20 -07:00