9.2.0

Signed-off-by: William Woodall <william@osrfoundation.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,27 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+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>`_)

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
+   * These callbacks will be called in the order they are added as the second
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * 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> &
+  virtual
-  get_on_shutdown_callbacks() const;
+  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
+   * Returned callbacks are a copy of the registered callbacks.
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
    */
   RCLCPP_PUBLIC
-  std::vector<OnShutdownCallback> &
+  std::vector<OnShutdownCallback>
-  get_on_shutdown_callbacks();
+  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::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
-  std::mutex on_shutdown_callbacks_mutex_;
+  mutable std::mutex on_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;

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.
+/// Thrown when an uninitialized parameter is accessed.
-class NoParameterOverrideProvided : public std::runtime_error
+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)
+  explicit ParameterUninitializedException(const std::string & name)
-  : std::runtime_error("parameter '" + name + "' requires an user provided parameter override")
+  : std::runtime_error("parameter '" + name + "' is not initialized")
   {}
 };
 

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

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<
+        auto subscription = std::dynamic_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+          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<
+        auto subscription = std::dynamic_pointer_cast<
-          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+          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

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.
+   * This function only considers services - not clients.
-   * \param[in] namespace_ the namespace of the node associated with the name
+   * The returned names are the actual names used and do not have remap rules applied.
-   * \return number of publishers that are advertised on a given topic.
+   *
-   * \throws std::runtime_error if publishers could not be counted
+   * \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>>

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,

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;

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 (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
-        if (handle) {
+          RCLCPP_ERROR(
-          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
-            RCLCPP_ERROR(
+            "Error in destruction of rcl service handle: %s",
-              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            rcl_get_error_string().str);
-              "Error in destruction of rcl service handle: %s",
+          rcl_reset_error();
-              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");
         }
         delete service;
       });

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_

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

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> &
+rclcpp::OnShutdownCallbackHandle
-Context::get_on_shutdown_callbacks() const
+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> &
+bool
-Context::get_on_shutdown_callbacks()
+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>

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();
+      rclcpp::node_interfaces::global_for_each_callback_group(
-      std::for_each(
+        node.get(),
-        group_ptrs.begin(), group_ptrs.end(),
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
         {
-          auto shared_group_ptr = group_ptr.lock();
+          if (
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
+            shared_group_ptr->automatically_add_to_executor_with_node() &&
-          !shared_group_ptr->get_associated_with_executor_atomic().load())
+            !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()) {
+  rclcpp::node_interfaces::global_for_each_callback_group(
-    auto group_ptr = weak_group.lock();
+    node_ptr.get(),
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
+    [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      group_ptr->automatically_add_to_executor_with_node())
     {
-      this->add_callback_group_to_map(
+      if (!group_ptr->get_associated_with_executor_atomic().load() &&
-        group_ptr,
+      group_ptr->automatically_add_to_executor_with_node())
-        node_ptr,
+      {
-        weak_groups_to_nodes_associated_with_executor_,
+        this->add_callback_group_to_map(
-        notify);
+          group_ptr,
-    }
+          node_ptr,
-  }
+          weak_groups_to_nodes_associated_with_executor_,
+          notify);
+      }
+    });
+
   weak_nodes_.push_back(node_ptr);
 }
 

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()) {
+  rclcpp::node_interfaces::global_for_each_callback_group(
-    auto group_ptr = weak_group.lock();
+    node_ptr.get(),
-    if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
+    [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      group_ptr->automatically_add_to_executor_with_node())
     {
-      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();
+      rclcpp::node_interfaces::global_for_each_callback_group(
-      std::for_each(
+        node.get(),
-        group_ptrs.begin(), group_ptrs.end(),
+        [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
-        [this, node](rclcpp::CallbackGroup::WeakPtr group_ptr)
         {
-          auto shared_group_ptr = group_ptr.lock();
+          if (shared_group_ptr->automatically_add_to_executor_with_node() &&
-          if (shared_group_ptr && shared_group_ptr->automatically_add_to_executor_with_node() &&
           !shared_group_ptr->get_associated_with_executor_atomic().load())
           {
             add_callback_group(

rclcpp/src/rclcpp/node.cpp

@@ -605,3 +605,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);
+}

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;
+  auto group = std::make_shared<rclcpp::CallbackGroup>(
-  using rclcpp::CallbackGroupType;
+    group_type,
-  auto group = CallbackGroup::SharedPtr(
+    automatically_add_to_executor_with_node);
-    new CallbackGroup(
+  auto mutex_ptr = NodeBase::map_object.get_mutex_of_nodebase(this);
-      group_type,
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
-      automatically_add_to_executor_with_node));
   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);
+    }
+  }
+}

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)) {
+  auto param_iter = parameters_.find(name);
-    return parameter;
+  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;
+    return rclcpp::Parameter{};
-  } else {
+  } else if (parameters_.end() == param_iter) {
     throw rclcpp::exceptions::ParameterNotDeclaredException(name);
+  } else {
+    throw rclcpp::exceptions::ParameterUninitializedException(name);
   }
 }
 

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)

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()) {
+  node->for_each_callback_group(
-    auto group = weak_group.lock();
+    [&number_of_entities](rclcpp::CallbackGroup::SharedPtr group)
-    EXPECT_NE(nullptr, group);
+    {
-    if (!group || !group->can_be_taken_from().load()) {
+      if (!group->can_be_taken_from().load()) {
-      return nullptr;
+        return;
-    }
+      }
-    group->find_subscription_ptrs_if(
+      group->find_subscription_ptrs_if(
-      [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
+        [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &)
-      {
+        {
-        number_of_entities->subscriptions++; return false;
+          number_of_entities->subscriptions++; return false;
-      });
+        });
-    group->find_timer_ptrs_if(
+      group->find_timer_ptrs_if(
-      [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
+        [&number_of_entities](rclcpp::TimerBase::SharedPtr &)
-      {
+        {
-        number_of_entities->timers++; return false;
+          number_of_entities->timers++; return false;
-      });
+        });
-    group->find_service_ptrs_if(
+      group->find_service_ptrs_if(
-      [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
+        [&number_of_entities](rclcpp::ServiceBase::SharedPtr &)
-      {
+        {
-        number_of_entities->services++; return false;
+          number_of_entities->services++; return false;
-      });
+        });
-    group->find_client_ptrs_if(
+      group->find_client_ptrs_if(
-      [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
+        [&number_of_entities](rclcpp::ClientBase::SharedPtr &)
-      {
+        {
-        number_of_entities->clients++; return false;
+          number_of_entities->clients++; return false;
-      });
+        });
-    group->find_waitable_ptrs_if(
+      group->find_waitable_ptrs_if(
-      [&number_of_entities](rclcpp::Waitable::SharedPtr &)
+        [&number_of_entities](rclcpp::Waitable::SharedPtr &)
-      {
+        {
-        number_of_entities->waitables++; return false;
+          number_of_entities->waitables++; return false;
-      });
+        });
-  }
+    });
 
   return number_of_entities;
 }

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()) {
+    node->for_each_callback_group(
-      auto group = group_weak_ptr.lock();
+      [](rclcpp::CallbackGroup::SharedPtr group)
-      if (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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes,
-      &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    basic_node->for_each_callback_group(
-    std::for_each(
+      [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes,
-      &basic_node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node_with_entity1->for_each_callback_group(
-    std::for_each(
+      [node_with_entity1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups1.begin(), callback_groups1.end(),
+      {
-      [&weak_groups_to_nodes,
-      &node_with_entity1](rclcpp::CallbackGroup::WeakPtr weak_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();
+    basic_node2->for_each_callback_group(
-    std::for_each(
+      [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups2.begin(), callback_groups2.end(),
+      {
-      [&weak_groups_to_uncollected_nodes,
-      &basic_node2](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node_with_entity2->for_each_callback_group(
-    std::for_each(
+      [node_with_entity2,
-      callback_groups3.begin(), callback_groups3.end(),
+      &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      [&weak_groups_to_uncollected_nodes,
+      {
-      &node_with_entity2](rclcpp::CallbackGroup::WeakPtr weak_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();
+    rclcpp::node_interfaces::global_for_each_callback_group(
-    std::for_each(
+      basic_node_base.get(),
-      callback_groups.begin(), callback_groups.end(),
+      [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      [&weak_groups_to_nodes, &basic_node_base](rclcpp::CallbackGroup::WeakPtr weak_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();
+    rclcpp::node_interfaces::global_for_each_callback_group(
-    std::for_each(
+      node_base.get(),
-      callback_groups.begin(), callback_groups.end(),
+      [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      [&weak_groups_to_nodes, &node_base](rclcpp::CallbackGroup::WeakPtr weak_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();
+  basic_node->for_each_callback_group(
-  std::for_each(
+    [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    callback_groups.begin(), callback_groups.end(),
+    {
-    [&weak_groups_to_nodes, &basic_node](rclcpp::CallbackGroup::WeakPtr weak_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();
+  node->for_each_callback_group(
-  std::for_each(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    callback_groups.begin(), callback_groups.end(),
+    {
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+  node->for_each_callback_group(
-  std::for_each(
+    [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-    callback_groups.begin(), callback_groups.end(),
+    {
-    [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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);

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:

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);
+}

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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    rclcpp::node_interfaces::global_for_each_callback_group(
-    std::for_each(
+      node_handle.get(),
-      callback_groups.begin(), callback_groups.end(),
+      [node_handle, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      [&weak_groups_to_nodes, &node_handle](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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();
+    node->for_each_callback_group(
-    std::for_each(
+      [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr)
-      callback_groups.begin(), callback_groups.end(),
+      {
-      [&weak_groups_to_nodes, &node](rclcpp::CallbackGroup::WeakPtr weak_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);

rclcpp/test/rclcpp/test_node.cpp

@@ -333,9 +333,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 +2642,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 +2786,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),
+      node->get_parameter("integer_override_not_given"),
-      rclcpp::exceptions::NoParameterOverrideProvided);
+      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(

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);
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,23 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+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)
 ------------------
 

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 (goal_handle) {
-    if (element != goal_handles_.end()) {
+      resp = handle_cancel_(goal_handle);
-      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
+      if (CancelResponse::ACCEPT == resp) {
-      if (goal_handle) {
+        try {
-        resp = handle_cancel_(goal_handle);
-        if (CancelResponse::ACCEPT == resp) {
           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;
         }
       }
     }

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.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>

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
+    // if there are clients who already asked for the result, send it to them
-  auto iter = pimpl_->result_requests_.find(uuid);
+    auto iter = pimpl_->result_requests_.find(uuid);
-  if (iter != pimpl_->result_requests_.end()) {
+    if (iter != pimpl_->result_requests_.end()) {
-    for (auto & request_header : iter->second) {
       std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-      rcl_ret_t ret = rcl_action_send_result_response(
+      for (auto & request_header : iter->second) {
-        pimpl_->action_server_.get(), &request_header, result_msg.get());
+        rcl_ret_t ret = rcl_action_send_result_response(
-      if (RCL_RET_OK != ret) {
+          pimpl_->action_server_.get(), &request_header, result_msg.get());
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
       }
     }
   }

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)

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();
+}

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+9.2.0 (2021-09-17)
+------------------
+
+9.1.0 (2021-05-20)
+------------------
+
+9.0.3 (2021-05-10)
+------------------
+
 9.0.2 (2021-04-14)
 ------------------
 

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

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,19 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+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)
 ------------------
 

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.

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

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

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();
   }
 

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();
+  size_t num_groups = 0;
-  EXPECT_EQ(groups.size(), 1u);
+  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();
+  num_groups = 0;
-  EXPECT_EQ(groups.size(), 2u);
+  test_node->for_each_callback_group(
-  EXPECT_EQ(groups[1].lock().get(), group.get());
+    [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr)
+    {
+      (void)group_ptr;
+      num_groups++;
+    });
+  EXPECT_EQ(num_groups, 2u);
 }
 
 TEST_F(TestDefaultStateMachine, wait_for_graph_change)